Dumb Bot (AI Robot Platform)

1#include <NewPing.h>
2#include <CircularBuffer.hpp>
3#include <Adafruit_MotorShield.h>
4
5// DC MOTORS
6Adafruit_MotorShield AFMS = Adafruit_MotorShield();
7Adafruit_DCMotor *Left_Motor = AFMS.getMotor(1);
8Adafruit_DCMotor *Right_Motor = AFMS.getMotor(2);
9
10int L_Motor_Speed = 0;
11int R_Motor_Speed = 0;
12
13//ECHO SENSOR's
14#define MAX_DISTANCE 900 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
15
16NewPing Front_Right_Sonar(12, 12, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
17NewPing Rear_Right_Sonar(13, 13, MAX_DISTANCE); //RIGHT SONARS
18
19#define buff_size 5
20CircularBuffer<int,buff_size> FR_Sonar_Buffer;
21CircularBuffer<int,buff_size> BR_Sonar_Buffer;
22int FR_Sonar_Value = 0;
23int BR_Sonar_Value = 0;
24
25// SONAR FUCNTIONS
26void update(){
27  FR_Sonar_Buffer.push(Front_Right_Sonar.ping_cm());
28  delay(10); //delay to avoid interference with the sonars
29  BR_Sonar_Buffer.push(Rear_Right_Sonar.ping_cm());
30
31  for (int i = 0; i < buff_size; i++){
32    FR_Sonar_Value = FR_Sonar_Value + FR_Sonar_Buffer[i];
33    BR_Sonar_Value = BR_Sonar_Value + BR_Sonar_Buffer[i];
34  }
35
36  FR_Sonar_Value = FR_Sonar_Value/buff_size;
37  BR_Sonar_Value = BR_Sonar_Value/buff_size;
38}
39void dump_sonar_data(){
40  Serial.print("R: Front: ");
41  Serial.print(FR_Sonar_Value);
42  Serial.print(" Back: ");
43  Serial.print(BR_Sonar_Value);
44  Serial.println("cm");
45}
46
47//DRIVE FUNCTIONS
48int safe_drive_motors(){
49  R_Motor_Speed = R_Motor_Speed*1.17;
50  if(L_Motor_Speed > 200){
51    return -1;
52  }
53  if(R_Motor_Speed > 200){
54    return -1;
55  }
56    if(L_Motor_Speed < 0){
57    return -1;
58  }
59  if(R_Motor_Speed < 0){
60    return -1;
61  }
62  drive_motor(L_Motor_Speed, R_Motor_Speed);
63}
64void drive_motor ( int a, int b ){
65  Left_Motor->setSpeed(a);
66  Right_Motor->setSpeed(b);
67}
68void dump_motor_data(){
69  Serial.print("L ");
70  Serial.print(L_Motor_Speed);
71  Serial.print(" R: ");
72  Serial.println(R_Motor_Speed);
73}
74
75void setup() {
76  Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.
77  if (!AFMS.begin()) {         // create with the default frequency 1.6KHz
78    Serial.println("Could not find Motor Shield. Check wiring.");
79    while (1);
80  }      
81  Left_Motor->run(FORWARD);
82  Right_Motor->run(FORWARD);
83  pinMode(11, OUTPUT);
84  Serial.println("All Status's Updated");
85  delay(1000);
86}
87void die(){
88  drive_motor(0,0);
89  delay(99999);
90  
91}
92void debug_drive(int a){
93  drive_motor(a,a);
94  delay(99999);
95}
96void debug_drive(int a, int b){
97  drive_motor(a,b);
98  delay(99999);
99}
100//Test Deltas
101int delta = 0;
102
103void loop_(){
104  update();
105  dump_sonar_data();
106  //delay(100);
107}
108
109int kpl = 10;
110int kpr = 10;
111void loop() {
112  //die();
113  debug_drive(150,175); // Minimum drive speed arox 130
114  // so 150L = 175R
115  
116  dump_sonar_data();
117  update();
118
119  delta = int(FR_Sonar_Value) - int(BR_Sonar_Value);
120  //Serial.println(delta);
121  L_Motor_Speed = 130 + kpl*delta;
122  R_Motor_Speed = 130 - kpr*delta;
123  if(L_Motor_Speed > R_Motor_Speed){
124    digitalWrite(11,HIGH);
125  }else{
126    digitalWrite(11,LOW);
127  }
128  safe_drive_motors();
129
130 
131  //delay(29);                     // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
132}