idp/idp.ino at new-main · apple-phi/idp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
#include <Arduino.h>
#include <Adafruit_MotorShield.h>
#include "./src/robot.h"
#include "./src/motors.h"
#include "./src/constants.h"
#include "./src/sensor.h"
#include "./src/led.h"

Robot *robot;
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *left = AFMS.getMotor(4);
Adafruit_DCMotor *right = AFMS.getMotor(3);
Servo arm;
Servo claw;
Sensors::Button *startButton;

void beginMotors()
{
    if (!AFMS.begin())
    {
        Serial.println("Could not find Motor Shield. Check wiring.");
        while (1)
            ;
    }
    Serial.println("Motor Shield found.");
}

void handleStartButton()
{
    startButton = new Sensors::Button(7);
    robot->servos.reset();
    while (!startButton->pressed())
    {
        delay(10);
    }
    delay(500);
}

void reset()
{
    delete robot;
    robot = new Robot({left, right}, {13, 12, 11, 10});
    asm volatile("jmp 0");
}

void setup()
{
    Serial.begin(9600);
    beginMotors();


    robot = new Robot({left, right}, {13, 12, 11, 10});

    Direction::nav_matrix[robot->latestNode][robot->currentDirection] = 1;

    delay(500);
    handleStartButton();
}

void loop()
{
    if (startButton->pressed())
    {
        reset();
    }
    (*robot)
        .readSensors()
        .drive()
        .delayAndBlinkIfMoving(1000 * DT);
}