Good day!
This is the original code that I inherited, and that I’ve been using as a reference as I write code for the neurorobot_reboot. In particular, I really wanted to simplify, simplify, simplify, and use shorter (clearer) variables along with comments so the code as a whole is easier to understand. I hadn’t really done much fancy stuff beyond methods before, though, so inheriting this has taught me some cool new techniques, like how to create classes in Arduino. 😀
So I’m just going to paste it in…
//NEUROROBOT_ORIGINAL_CODE
//#include <EnableInterrupt.h>
// Serial: 0 (RX) and 1 (TX); Serial 1: 19 (RX) and 18 (TX);
// Serial 2: 17 (RX) and 16 (TX); Serial 3: 15 (RX) and 14 (TX).
// External Interrupts: 2 (interrupt 0), 3 (interrupt 1),
// 18 (interrupt 5), 19 (interrupt 4), 20 (interrupt 3), and 21 (interrupt 2)
// PWM: 2 to 13 and 44 to 46
// SPI: 50 (MISO), 51 (MOSI), 52 (SCK), 53 (SS)
// LED: 13
// I2C: 20 (SDA) and 21 (SCL)
#define COMMAND_START '@'
#define PARAMETER_SEPARATOR ':'
#define PIN_LED 13
#define PIN_RIGHT_ENABLE 7
#define PIN_RIGHT_FORWARD 5
#define PIN_RIGHT_BACKWARD 6
#define PIN_LEFT_ENABLE 2
#define PIN_LEFT_FORWARD 3
#define PIN_LEFT_BACKWARD 4
//#define PIN_LEFT_ECHO 24
//#define PIN_LEFT_PING 23
//#define PIN_FRONT_ECHO 26
//#define PIN_FRONT_PING 25
//#define PIN_RIGHT_ECHO 28
//#define PIN_RIGHT_PING 27
#define PIN_RIGHT_ECHO 30
#define PIN_RIGHT_PING 29
#define PIN_FRONT_ECHO 32
#define PIN_FRONT_PING 31
#define PIN_LEFT_ECHO 33
#define PIN_LEFT_PING 34
#define PIN_HALL_R_A 44
#define PIN_HALL_R_B 45
#define PIN_HALL_L_A 46
#define PIN_HALL_L_B 47
#define SPEED_LIMIT 60
#define TURN_SPEED 120
#define RANGE_SHORT 25
#define RANGE_MED 35
#define RANGE_LONG 45
boolean DEBUG = false;
class Command {
public:
int right_speed;
int left_speed;
Command() :right_speed(0), left_speed(0)
{}
void parseCommand() {
while(char inchar = Serial.read() != COMMAND_START) {}
right_speed = Serial.parseInt();
left_speed = Serial.parseInt();
Serial.read();//carriage return
Serial.read();//newline
}
};
class Motor {
private:
int pin_forward;
int pin_backward;
int pin_enable;
int speed;
public:
Motor(int forward, int backward, int enable) :pin_forward(forward), pin_backward(backward), pin_enable(enable) {
pinMode(pin_forward, OUTPUT);
pinMode(pin_backward, OUTPUT);
pinMode(pin_enable, OUTPUT);
}
void setSpeed(int speed) {
if(speed > SPEED_LIMIT) speed = SPEED_LIMIT;
if(speed < -SPEED_LIMIT) speed = -SPEED_LIMIT;
if(speed > 0) {
digitalWrite(pin_backward, LOW);
digitalWrite(pin_forward, HIGH);
analogWrite(pin_enable, speed);
} else {
digitalWrite(pin_forward, LOW);
digitalWrite(pin_backward, HIGH);
analogWrite(pin_enable, -speed);
}
this->speed = speed;
}
int getSpeed() {
return speed;
}
};
class Robot {
private:
Motor motor_right;
Motor motor_left;
public:
Robot(Motor right, Motor left) :motor_right(right), motor_left(left) {
}
void setSpeed(int right, int left) {
motor_right.setSpeed(right);
motor_left.setSpeed(left);
}
};
class PingSensor {
private:
int pin_echo;
long ping_time;
int pin_ping;
void trigger() {
digitalWrite(pin_ping, LOW);
delayMicroseconds(1);
digitalWrite(pin_ping, HIGH);
delayMicroseconds(2);
digitalWrite(pin_ping, LOW);
}
public:
PingSensor(int ping, int echo) :pin_ping(ping), pin_echo(echo) {
pinMode(pin_ping, OUTPUT);
pinMode(pin_echo, INPUT);
}
void ping();
int distance(void) {
return ping_time/29/2;
}
};
void PingSensor::ping() {
trigger();
ping_time = pulseIn(pin_echo, HIGH, 3000);
if(ping_time == 0) ping_time = 3000;
}
Motor rightMotor(PIN_RIGHT_FORWARD, PIN_RIGHT_BACKWARD, PIN_RIGHT_ENABLE);
Motor leftMotor(PIN_LEFT_FORWARD, PIN_LEFT_BACKWARD, PIN_LEFT_ENABLE);
Robot myBot(rightMotor,leftMotor);
Command incoming;
PingSensor leftSensor(PIN_LEFT_PING, PIN_LEFT_ECHO);
PingSensor rightSensor(PIN_RIGHT_PING, PIN_RIGHT_ECHO);
PingSensor frontSensor(PIN_FRONT_PING, PIN_FRONT_ECHO);
//setup() runs once on powerup
void setup() {
pinMode(PIN_LED, OUTPUT);
pinMode(PIN_HALL_R_A, INPUT);
pinMode(PIN_HALL_R_B, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_HALL_R_A), rightup, RISING);
attachInterrupt(digitalPinToInterrupt(PIN_HALL_R_A), rightdown, FALLING);
pinMode(PIN_HALL_L_A, INPUT);
pinMode(PIN_HALL_L_B, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_HALL_L_A), leftup, RISING);
attachInterrupt(digitalPinToInterrupt(PIN_HALL_L_A), leftdown, FALLING);
//Begin serial communications
Serial.begin(38400);
Serial.println("Starting up...");
Serial.setTimeout(3);//Set timeout to 1 ms so parseInt doesnt wait forever after the last digit
digitalWrite(13, HIGH);//Indicate we are running main loop by lighting onboard led
delay(3000);
rightSensor.ping();
leftSensor.ping();
frontSensor.ping();
//This will allow robot to move without Matlab sending commands,
//by holding hand in front of left and front sensors during startup
if(leftSensor.distance() < 10 && frontSensor.distance() < 10 && rightSensor.distance() > 20) {
incoming.right_speed = 70;
incoming.left_speed = 90;
}
}
// ToDo: Handle rollover
uint32_t startTime;
// This is main code loop, this code runs after setup() and repeats forever
void loop() {
digitalWrite(PIN_LED, !digitalRead(PIN_LED));//flash board led when signals are transceived
//Avoid obstacles while commands are not being recieved
while(!Serial.available()) {
getRPS(); // Update RPS counters
//Keep it moving straight forward, after the obstical avoidance block is left
myBot.setSpeed(incoming.right_speed,255);
delay(4);
myBot.setSpeed(incoming.right_speed, incoming.left_speed);
delay(10);
///Get distance on all sensors
leftSensor.ping();
frontSensor.ping();
rightSensor.ping();
//#define RANGE_SHORT 20
//#define RANGE_MED 25
//#define RANGE_LONG 30
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//Obstacal avoidance
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if(leftSensor.distance() < RANGE_SHORT && rightSensor.distance() > RANGE_SHORT && frontSensor.distance() > RANGE_MED) {
myBot.setSpeed(-TURN_SPEED, TURN_SPEED);
while(leftSensor.distance() < RANGE_SHORT && !Serial.available()) {
leftSensor.ping();
if(DEBUG) Serial.println("CASE1");
}
} else if (rightSensor.distance() < RANGE_SHORT && leftSensor.distance() > RANGE_SHORT && frontSensor.distance() > RANGE_MED) {
myBot.setSpeed(TURN_SPEED, -TURN_SPEED);
while(rightSensor.distance() < RANGE_SHORT && !Serial.available()) {
rightSensor.ping();
if(DEBUG) Serial.println("CASE2");
}
} else if (frontSensor.distance() < RANGE_MED && rightSensor.distance() > RANGE_SHORT && leftSensor.distance() > RANGE_SHORT) {
myBot.setSpeed(0, -TURN_SPEED);
while(frontSensor.distance() < RANGE_LONG && !Serial.available()) {
frontSensor.ping();
if(DEBUG) Serial.println("CASE3");
}
} else if(frontSensor.distance() < RANGE_MED && rightSensor.distance() < RANGE_SHORT && leftSensor.distance() > RANGE_SHORT) {
myBot.setSpeed(TURN_SPEED, -TURN_SPEED);
while((frontSensor.distance() < RANGE_LONG || rightSensor.distance() < RANGE_MED) && !Serial.available()) {
frontSensor.ping();
rightSensor.ping();
if(DEBUG) Serial.println("CASE4");
}
} else if(frontSensor.distance() < RANGE_MED && leftSensor.distance() < RANGE_SHORT && rightSensor.distance() > RANGE_SHORT) {
myBot.setSpeed(-TURN_SPEED, TURN_SPEED);
while((frontSensor.distance() < RANGE_LONG || leftSensor.distance() < RANGE_MED) && !Serial.available()) {
frontSensor.ping();
leftSensor.ping();
if(DEBUG) Serial.println("CASE5");
}
} else if(frontSensor.distance() < RANGE_LONG && leftSensor.distance() < RANGE_MED && rightSensor.distance() < RANGE_MED) {
myBot.setSpeed(-(TURN_SPEED-30), -TURN_SPEED);
while((frontSensor.distance() < RANGE_LONG || leftSensor.distance() < RANGE_LONG || rightSensor.distance() < RANGE_LONG) && !Serial.available()) {
leftSensor.ping();
frontSensor.ping();
rightSensor.ping();
if(DEBUG) Serial.println("CASE6");
}
}
if(DEBUG) {
printSensors();
printSpeed();
}
}//end while Serial!Avail
//Communicate with Matlab
incoming.parseCommand();//Get command
//Carry out command
myBot.setSpeed(255, 255);
delay(3);
myBot.setSpeed(incoming.right_speed, incoming.left_speed);
delay(10);
//Send response
printSensors();
//Refresh distances, in case a command immediately follows
leftSensor.ping();
rightSensor.ping();
frontSensor.ping();
}
// Count hits on the hall sensor
// Must be declared volatile, as they are modified in interrupt service routine (ISR)
// ToDo: Handle rollover
volatile int32_t count_right = 0;
volatile int32_t count_left = 0;
// Called on rising edge of PIN_HALL_R_A
void rightup() {
if(digitalRead(PIN_HALL_R_B)) // Work out direction of rotation
count_right--;
else
count_right++;
}
// Called on rising edge of PIN_HALL_R_A
void rightdown() {
if(digitalRead(PIN_HALL_R_B)) // Work out direction of rotation
count_right++;
else
count_right--;
}
// Called on rising edge of PIN_HALL_L_A
void leftup() {
if(digitalRead(PIN_HALL_L_B)) // Work out direction of rotation
count_left--;
else
count_left++;
}
// Called on falling edge of PIN_HALL_L_A
void leftdown() {
if(digitalRead(PIN_HALL_L_B)) // Work out direction of rotation
count_right++;
else
count_left--;
}
uint32_t lastCount_r = 0, lastCount_l = 0;
int16_t rps_r = 0, rps_l = 0;
uint32_t lastTime = 0;
void getRPS() {
uint32_t currentTime = micros();
// Update at 5Hz
if(currentTime - lastTime < 200000) { // 200ms
return;
}
// Calculate counts per second on hall sensor
float cps_r = ((float)(count_right - lastCount_r)) / ((float)(currentTime - lastTime) / 1000000);
float cps_l = ((float)(count_left - lastCount_l)) / ((float)(currentTime - lastTime) / 1000000);
// Calculate rotations per second on output shaft
rps_r = cps_r/480;
rps_l = cps_l/480;
lastTime = currentTime;
lastCount_r = count_right;
lastCount_l = count_left;
}
void printSensors() {
Serial.print("Dist: [ ");
Serial.print(leftSensor.distance());
Serial.print(" ");
Serial.print(frontSensor.distance());
Serial.print(" ");
Serial.print(rightSensor.distance());
Serial.println(" ]");
}
void printSpeed() {
Serial.print("RPS: < ");
Serial.print(rps_l);
Serial.print(" ");
Serial.print(rps_r);
Serial.println(" >");
}