Blake Babbs / WarehouseBotOther

testing

Dependencies:   btbee m3pi_ng mbed

Fork of WarehouseBot by Blake Babbs

Robot-bae8eb81a9d7/main.cpp@3:4284d7cdb68e, 2015-06-04 (annotated)

Committer:
bbabbs
Date:
Thu Jun 04 12:18:04 2015 +0000
Revision:
3:4284d7cdb68e
Parent:
2:2d147091491d
Child:
4:a9823e6dab12
for bluetooth

Who changed what in which revision?

UserRevisionLine numberNew contents of line
charwhit 0:0f96a93fbf39 1 #include "mbed.h"
charwhit 0:0f96a93fbf39 2 #include "btbee.h"
charwhit 0:0f96a93fbf39 3 #include "m3pi_ng.h"
charwhit 0:0f96a93fbf39 4 #include <vector>
charwhit 0:0f96a93fbf39 5 #include <string>
charwhit 0:0f96a93fbf39 6 m3pi robot;
charwhit 0:0f96a93fbf39 7 Timer timer;
charwhit 0:0f96a93fbf39 8 Timer time_wait;
bbabbs 2:2d147091491d 9 #define MAX 0.5
charwhit 0:0f96a93fbf39 10 #define MIN 0
charwhit 0:0f96a93fbf39 11
charwhit 0:0f96a93fbf39 12 #define P_TERM 2.5
charwhit 0:0f96a93fbf39 13 #define I_TERM 2
charwhit 0:0f96a93fbf39 14 #define D_TERM 23
charwhit 0:0f96a93fbf39 15
charwhit 0:0f96a93fbf39 16 void turnRight();
charwhit 0:0f96a93fbf39 17 void turnLeft();
charwhit 0:0f96a93fbf39 18 void goStraight();
charwhit 0:0f96a93fbf39 19
charwhit 0:0f96a93fbf39 20 int main()
charwhit 0:0f96a93fbf39 21 {
charwhit 0:0f96a93fbf39 22
charwhit 0:0f96a93fbf39 23 robot.sensor_auto_calibrate();
charwhit 0:0f96a93fbf39 24 wait(2.0);
charwhit 0:0f96a93fbf39 25
charwhit 0:0f96a93fbf39 26 float right;
charwhit 0:0f96a93fbf39 27 float left;
charwhit 0:0f96a93fbf39 28 //float current_pos[5];
charwhit 0:0f96a93fbf39 29 float current_pos = 0.0;
charwhit 0:0f96a93fbf39 30 float previous_pos =0.0;
charwhit 0:0f96a93fbf39 31 float derivative, proportional, integral = 0;
charwhit 0:0f96a93fbf39 32 float power;
charwhit 0:0f96a93fbf39 33 float speed = MAX;
charwhit 0:0f96a93fbf39 34 string turns;
charwhit 0:0f96a93fbf39 35 int direction = 0;
charwhit 0:0f96a93fbf39 36 int x [5];
charwhit 0:0f96a93fbf39 37 bool passed = false;
charwhit 0:0f96a93fbf39 38
charwhit 0:0f96a93fbf39 39
charwhit 0:0f96a93fbf39 40
charwhit 0:0f96a93fbf39 41 int Run = 1;
bbabbs 3:4284d7cdb68e 42 string blah[36] = {"", "LRRL", "SSRL", "RLSRL", "RLLLRSRLLR", "LRLL",
charwhit 1:2db3ccba18c8 43 "RLLR", "", "SSR", "RLRR", "RLLRLSR", "LRSLR",
charwhit 1:2db3ccba18c8 44 "RLSS", "LLS", "", "RR", "RLSLR", "LRSSS",
charwhit 1:2db3ccba18c8 45 "RLSRL", "LLRL", "LL", "", "RLSS", "RLRLS",
charwhit 1:2db3ccba18c8 46 "RRSRR", "LSRLRRL", "LRSRL", "SSRL", "", "RLRRRL",
charwhit 1:2db3ccba18c8 47 "RRR", "LRSLR", "SSSLR", "SRLL", "RLLLRL", ""};
charwhit 1:2db3ccba18c8 48 //vector< vector<string> > Lookup(6, vector<string>(6));
charwhit 0:0f96a93fbf39 49
charwhit 1:2db3ccba18c8 50 //Lookup[1][2] = "LLS";
charwhit 0:0f96a93fbf39 51
charwhit 0:0f96a93fbf39 52
charwhit 0:0f96a93fbf39 53
charwhit 0:0f96a93fbf39 54
charwhit 0:0f96a93fbf39 55
charwhit 0:0f96a93fbf39 56 time_wait.start();
charwhit 0:0f96a93fbf39 57
bbabbs 3:4284d7cdb68e 58
charwhit 0:0f96a93fbf39 59 while(Run) {
charwhit 0:0f96a93fbf39 60 time_wait.reset();
charwhit 0:0f96a93fbf39 61 //Get raw sensor values
charwhit 0:0f96a93fbf39 62
charwhit 0:0f96a93fbf39 63 robot.calibrated_sensor(x);
charwhit 0:0f96a93fbf39 64
bbabbs 3:4284d7cdb68e 65 //insert end and beginning locations
charwhit 0:0f96a93fbf39 66
bbabbs 3:4284d7cdb68e 67 turns = blah[(end-1)*6+(start-1)];
charwhit 0:0f96a93fbf39 68
charwhit 0:0f96a93fbf39 69 //Check to make sure battery isn't low
charwhit 0:0f96a93fbf39 70 if (robot.battery() < 2.4) {
charwhit 0:0f96a93fbf39 71 timer.stop();
charwhit 0:0f96a93fbf39 72 break;
charwhit 0:0f96a93fbf39 73 }
charwhit 0:0f96a93fbf39 74
charwhit 0:0f96a93fbf39 75
charwhit 1:2db3ccba18c8 76
charwhit 0:0f96a93fbf39 77 if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
charwhit 0:0f96a93fbf39 78
charwhit 1:2db3ccba18c8 79
charwhit 0:0f96a93fbf39 80 if(turns.at(direction) == 'L'){
bbabbs 2:2d147091491d 81 while(x[0] > 300){
bbabbs 2:2d147091491d 82 goStraight();
bbabbs 2:2d147091491d 83 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 84 }
charwhit 1:2db3ccba18c8 85 previous_pos = 0;
charwhit 0:0f96a93fbf39 86 robot.stop();
charwhit 1:2db3ccba18c8 87 //wait(1);
bbabbs 3:4284d7cdb68e 88 while(x[2] > 300){
bbabbs 2:2d147091491d 89 turnLeft();
bbabbs 2:2d147091491d 90 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 91 }
bbabbs 2:2d147091491d 92
bbabbs 2:2d147091491d 93 while((x[0] > 300) || (x[2] < 300)){
bbabbs 2:2d147091491d 94 turnLeft();
bbabbs 2:2d147091491d 95 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 96 }
charwhit 0:0f96a93fbf39 97 robot.stop();
charwhit 1:2db3ccba18c8 98 //continue;
charwhit 1:2db3ccba18c8 99 /*wait(1);
charwhit 0:0f96a93fbf39 100 goStraight();
charwhit 1:2db3ccba18c8 101 wait(1);*/
charwhit 0:0f96a93fbf39 102 }
charwhit 0:0f96a93fbf39 103 else if(turns.at(direction) == 'R'){
charwhit 1:2db3ccba18c8 104 previous_pos = 0;
charwhit 0:0f96a93fbf39 105 robot.stop();
bbabbs 3:4284d7cdb68e 106 while(x[4] > 300){
bbabbs 3:4284d7cdb68e 107 goStraight();
bbabbs 3:4284d7cdb68e 108 robot.calibrated_sensor(x);
bbabbs 3:4284d7cdb68e 109 }
charwhit 1:2db3ccba18c8 110 //wait(1);
bbabbs 3:4284d7cdb68e 111 while(x[2] > 300){
bbabbs 2:2d147091491d 112 turnRight();
bbabbs 2:2d147091491d 113 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 114 }
bbabbs 2:2d147091491d 115
bbabbs 2:2d147091491d 116 while((x[4] > 300) || (x[2] < 300)){
bbabbs 2:2d147091491d 117 turnRight();
bbabbs 2:2d147091491d 118 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 119 }
charwhit 0:0f96a93fbf39 120 robot.stop();
charwhit 1:2db3ccba18c8 121
charwhit 1:2db3ccba18c8 122 /*wait(1);
charwhit 0:0f96a93fbf39 123 goStraight();
charwhit 1:2db3ccba18c8 124 wait(1);*/
charwhit 0:0f96a93fbf39 125 }
charwhit 0:0f96a93fbf39 126 else{
charwhit 0:0f96a93fbf39 127 robot.stop();
bbabbs 2:2d147091491d 128 //wait(1);
bbabbs 2:2d147091491d 129 while(x[0] > 300 || x[4] > 300){
bbabbs 2:2d147091491d 130 goStraight();
bbabbs 2:2d147091491d 131 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 132 }
charwhit 1:2db3ccba18c8 133 //wait(1);
charwhit 0:0f96a93fbf39 134 }
charwhit 1:2db3ccba18c8 135 robot.cls();
charwhit 1:2db3ccba18c8 136 robot.printf("Size %d", direction);
charwhit 0:0f96a93fbf39 137 ++direction;
charwhit 1:2db3ccba18c8 138 passed = true;
charwhit 1:2db3ccba18c8 139 continue;
charwhit 0:0f96a93fbf39 140 }
charwhit 0:0f96a93fbf39 141 else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
charwhit 0:0f96a93fbf39 142 robot.stop();
charwhit 1:2db3ccba18c8 143 robot.cls();
charwhit 1:2db3ccba18c8 144 robot.printf("Try again");
charwhit 0:0f96a93fbf39 145 break;
charwhit 0:0f96a93fbf39 146 }
charwhit 0:0f96a93fbf39 147 else if (x[0]>300 || x[4]>300 && passed)
charwhit 0:0f96a93fbf39 148 passed = true;
charwhit 0:0f96a93fbf39 149 else
charwhit 0:0f96a93fbf39 150 passed = false;
charwhit 0:0f96a93fbf39 151
charwhit 0:0f96a93fbf39 152
charwhit 0:0f96a93fbf39 153 // Get the position of the line.
charwhit 0:0f96a93fbf39 154 current_pos = robot.line_position();
charwhit 0:0f96a93fbf39 155 proportional = current_pos;
charwhit 0:0f96a93fbf39 156
charwhit 0:0f96a93fbf39 157 derivative = current_pos - previous_pos;
charwhit 0:0f96a93fbf39 158
charwhit 0:0f96a93fbf39 159
charwhit 0:0f96a93fbf39 160 //compute the integral
charwhit 0:0f96a93fbf39 161 integral =+ proportional;
charwhit 0:0f96a93fbf39 162
charwhit 0:0f96a93fbf39 163 //remember the last position.
charwhit 0:0f96a93fbf39 164 previous_pos = current_pos;
charwhit 0:0f96a93fbf39 165
charwhit 0:0f96a93fbf39 166 // compute the power
charwhit 0:0f96a93fbf39 167 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 0:0f96a93fbf39 168 //computer new speeds
charwhit 0:0f96a93fbf39 169 right = speed+power;
charwhit 0:0f96a93fbf39 170 left = speed-power;
charwhit 0:0f96a93fbf39 171
charwhit 0:0f96a93fbf39 172 //limit checks
charwhit 0:0f96a93fbf39 173 if(right<MIN)
charwhit 0:0f96a93fbf39 174 right = MIN;
charwhit 0:0f96a93fbf39 175 else if (right > MAX)
charwhit 0:0f96a93fbf39 176 right = MAX;
charwhit 0:0f96a93fbf39 177
charwhit 0:0f96a93fbf39 178 if(left<MIN)
charwhit 0:0f96a93fbf39 179 left = MIN;
charwhit 0:0f96a93fbf39 180 else if (left>MIN)
charwhit 0:0f96a93fbf39 181 left = MAX;
charwhit 0:0f96a93fbf39 182
charwhit 0:0f96a93fbf39 183 //set speed
charwhit 0:0f96a93fbf39 184
charwhit 0:0f96a93fbf39 185 robot.left_motor(left);
charwhit 0:0f96a93fbf39 186 robot.right_motor(right);
charwhit 0:0f96a93fbf39 187
charwhit 0:0f96a93fbf39 188 wait((5-time_wait.read_ms())/1000);
charwhit 0:0f96a93fbf39 189 }
charwhit 0:0f96a93fbf39 190
charwhit 0:0f96a93fbf39 191
charwhit 0:0f96a93fbf39 192
charwhit 0:0f96a93fbf39 193 robot.stop();
charwhit 0:0f96a93fbf39 194
charwhit 1:2db3ccba18c8 195 /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 0:0f96a93fbf39 196 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 0:0f96a93fbf39 197 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 0:0f96a93fbf39 198 ,'G','8','E','8','D','8','C','4'
charwhit 0:0f96a93fbf39 199 };
charwhit 0:0f96a93fbf39 200 int numb = 59;
charwhit 0:0f96a93fbf39 201
charwhit 1:2db3ccba18c8 202 robot.playtune(hail,numb);*/
charwhit 0:0f96a93fbf39 203
charwhit 0:0f96a93fbf39 204
charwhit 0:0f96a93fbf39 205 }
charwhit 0:0f96a93fbf39 206
charwhit 0:0f96a93fbf39 207
charwhit 0:0f96a93fbf39 208 void turnRight(){
bbabbs 2:2d147091491d 209 //Timer turner;
bbabbs 2:2d147091491d 210 //turner.start();
bbabbs 2:2d147091491d 211 //while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 212 robot.right(.5);
bbabbs 2:2d147091491d 213 //}
bbabbs 2:2d147091491d 214 //turner.stop();
charwhit 0:0f96a93fbf39 215 }
charwhit 0:0f96a93fbf39 216
charwhit 0:0f96a93fbf39 217 void turnLeft(){
bbabbs 2:2d147091491d 218 // Timer turner;
bbabbs 2:2d147091491d 219 // turner.start();
bbabbs 2:2d147091491d 220 // while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 221 robot.left(.5);
bbabbs 2:2d147091491d 222 // }
bbabbs 2:2d147091491d 223 //turner.stop();
charwhit 0:0f96a93fbf39 224 }
charwhit 0:0f96a93fbf39 225
charwhit 0:0f96a93fbf39 226 void goStraight(){
bbabbs 2:2d147091491d 227 // Timer turner;
bbabbs 2:2d147091491d 228 // turner.start();
bbabbs 2:2d147091491d 229 // while (turner.read_ms() < 5){
charwhit 0:0f96a93fbf39 230 robot.forward(.5);
bbabbs 2:2d147091491d 231 // }
bbabbs 2:2d147091491d 232 // turner.stop();
charwhit 0:0f96a93fbf39 233 }