RaceBot/Car.cpp at master · rswhiting/RaceBot · 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
71
72
73
74
75
76
77
78
79
80
81
82
83
#include "Car.h"

Car::Car() :
    leftSonarSensor(LEFT_TRIGGER_PIN, LEFT_ECHO_PIN, MAX_PING),
    forwardSonarSensor(FORWARD_TRIGGER_PIN, FORWARD_ECHO_PIN, MAX_PING),
    forwardMotor(4),
    turnMotor(3)
    {
    // init motors
    forwardMotor.setSpeed(MOTOR_SPEED);
    forwardMotor.run(RELEASE);
    turnMotor.setSpeed(MOTOR_SPEED);
    turnMotor.run(RELEASE);
    Serial.println("Motors initialized");
}

double Car::leftDist(){
    double distance = distance = leftSonarSensor.ping() / US_ROUNDTRIP_CM;
    // get rid of interference
    left = distance > MAX_PING ? 0 : distance;
    return left;
}

double Car::frontDist(){
    double distance = distance = forwardSonarSensor.ping() / US_ROUNDTRIP_CM;
    // get rid of interference
    front = distance > MAX_PING ? 0 : distance;
    return front;
}

void Car::goBack(){
    if(!FROZEN){
        forwardMotor.run(FORWARD);
        turnMotor.run(RELEASE);
    }
    action = "Back";
}

void Car::goForward(){
    if(!FROZEN){
        forwardMotor.run(BACKWARD);
        turnMotor.run(RELEASE);
    }
    action = "Forward";
}

void Car::goLeft(){
    if(!FROZEN){
        turnMotor.run(FORWARD);
    }
    action = "Left";
}

void Car::goRight(){
    if(!FROZEN){
        turnMotor.run(BACKWARD);
    }
    action = "Right";
}

void Car::stop(){
    if(!FROZEN){
        forwardMotor.run(RELEASE);
        turnMotor.run(RELEASE);
    }
    action = "Stop";
}

void Car::print(){
    Serial.print("left ");
    if(front == 0)
      Serial.print("----");
    else
      Serial.print(left);
    Serial.print(" front ");
    if(front == 0)
      Serial.print("----");
    else
      Serial.print(front);
    Serial.print(" action ");
    Serial.print(action);
    Serial.print("\n");
}