Blake Babbs / WarehouseBotOther

testing

Dependencies:   btbee m3pi_ng mbed

Fork of WarehouseBot by Blake Babbs

Robot-bae8eb81a9d7/main.cpp@4:a9823e6dab12, 2015-06-08 (annotated)

Committer:
bbabbs
Date:
Mon Jun 08 11:23:07 2015 +0000
Revision:
4:a9823e6dab12
Parent:
3:4284d7cdb68e 
kind of works;

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
bbabbs 4:a9823e6dab12 12 #define P_TERM 3
bbabbs 4:a9823e6dab12 13 #define I_TERM 0
bbabbs 4:a9823e6dab12 14 #define D_TERM 27
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();
bbabbs 4:a9823e6dab12 57 turns = blah[(6-1)*6+(5-1)];
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 4:a9823e6dab12 67
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
bbabbs 4:a9823e6dab12 77 if(((x[0]>300 && (turns.at(direction) == 'L')) || (x[4]>300 && (turns.at(direction) == 'R')) || ((x[0] > 300 || x[4] > 300) && turns.at(direction) == 'S')) && (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 4:a9823e6dab12 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");
bbabbs 4:a9823e6dab12 145 while(x[2] < 300 || x[0] < 300 || x[4] < 300){
bbabbs 4:a9823e6dab12 146 turnRight();
bbabbs 4:a9823e6dab12 147 robot.calibrated_sensor(x);
bbabbs 4:a9823e6dab12 148 }
charwhit 0:0f96a93fbf39 149 break;
charwhit 0:0f96a93fbf39 150 }
charwhit 0:0f96a93fbf39 151 else if (x[0]>300 || x[4]>300 && passed)
charwhit 0:0f96a93fbf39 152 passed = true;
charwhit 0:0f96a93fbf39 153 else
charwhit 0:0f96a93fbf39 154 passed = false;
charwhit 0:0f96a93fbf39 155
charwhit 0:0f96a93fbf39 156
charwhit 0:0f96a93fbf39 157 // Get the position of the line.
charwhit 0:0f96a93fbf39 158 current_pos = robot.line_position();
charwhit 0:0f96a93fbf39 159 proportional = current_pos;
charwhit 0:0f96a93fbf39 160
charwhit 0:0f96a93fbf39 161 derivative = current_pos - previous_pos;
charwhit 0:0f96a93fbf39 162
charwhit 0:0f96a93fbf39 163
charwhit 0:0f96a93fbf39 164 //compute the integral
charwhit 0:0f96a93fbf39 165 integral =+ proportional;
charwhit 0:0f96a93fbf39 166
charwhit 0:0f96a93fbf39 167 //remember the last position.
charwhit 0:0f96a93fbf39 168 previous_pos = current_pos;
charwhit 0:0f96a93fbf39 169
charwhit 0:0f96a93fbf39 170 // compute the power
charwhit 0:0f96a93fbf39 171 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 0:0f96a93fbf39 172 //computer new speeds
charwhit 0:0f96a93fbf39 173 right = speed+power;
charwhit 0:0f96a93fbf39 174 left = speed-power;
charwhit 0:0f96a93fbf39 175
charwhit 0:0f96a93fbf39 176 //limit checks
charwhit 0:0f96a93fbf39 177 if(right<MIN)
charwhit 0:0f96a93fbf39 178 right = MIN;
charwhit 0:0f96a93fbf39 179 else if (right > MAX)
charwhit 0:0f96a93fbf39 180 right = MAX;
charwhit 0:0f96a93fbf39 181
charwhit 0:0f96a93fbf39 182 if(left<MIN)
charwhit 0:0f96a93fbf39 183 left = MIN;
charwhit 0:0f96a93fbf39 184 else if (left>MIN)
charwhit 0:0f96a93fbf39 185 left = MAX;
charwhit 0:0f96a93fbf39 186
charwhit 0:0f96a93fbf39 187 //set speed
charwhit 0:0f96a93fbf39 188
charwhit 0:0f96a93fbf39 189 robot.left_motor(left);
charwhit 0:0f96a93fbf39 190 robot.right_motor(right);
charwhit 0:0f96a93fbf39 191
charwhit 0:0f96a93fbf39 192 wait((5-time_wait.read_ms())/1000);
charwhit 0:0f96a93fbf39 193 }
charwhit 0:0f96a93fbf39 194
charwhit 0:0f96a93fbf39 195
charwhit 0:0f96a93fbf39 196
charwhit 0:0f96a93fbf39 197 robot.stop();
charwhit 0:0f96a93fbf39 198
charwhit 1:2db3ccba18c8 199 /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 0:0f96a93fbf39 200 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 0:0f96a93fbf39 201 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 0:0f96a93fbf39 202 ,'G','8','E','8','D','8','C','4'
charwhit 0:0f96a93fbf39 203 };
charwhit 0:0f96a93fbf39 204 int numb = 59;
charwhit 0:0f96a93fbf39 205
charwhit 1:2db3ccba18c8 206 robot.playtune(hail,numb);*/
charwhit 0:0f96a93fbf39 207
charwhit 0:0f96a93fbf39 208
charwhit 0:0f96a93fbf39 209 }
charwhit 0:0f96a93fbf39 210
charwhit 0:0f96a93fbf39 211
charwhit 0:0f96a93fbf39 212 void turnRight(){
bbabbs 2:2d147091491d 213 //Timer turner;
bbabbs 2:2d147091491d 214 //turner.start();
bbabbs 2:2d147091491d 215 //while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 216 robot.right(.5);
bbabbs 2:2d147091491d 217 //}
bbabbs 2:2d147091491d 218 //turner.stop();
charwhit 0:0f96a93fbf39 219 }
charwhit 0:0f96a93fbf39 220
charwhit 0:0f96a93fbf39 221 void turnLeft(){
bbabbs 2:2d147091491d 222 // Timer turner;
bbabbs 2:2d147091491d 223 // turner.start();
bbabbs 2:2d147091491d 224 // while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 225 robot.left(.5);
bbabbs 2:2d147091491d 226 // }
bbabbs 2:2d147091491d 227 //turner.stop();
charwhit 0:0f96a93fbf39 228 }
charwhit 0:0f96a93fbf39 229
charwhit 0:0f96a93fbf39 230 void goStraight(){
bbabbs 2:2d147091491d 231 // Timer turner;
bbabbs 2:2d147091491d 232 // turner.start();
bbabbs 2:2d147091491d 233 // while (turner.read_ms() < 5){
charwhit 0:0f96a93fbf39 234 robot.forward(.5);
bbabbs 2:2d147091491d 235 // }
bbabbs 2:2d147091491d 236 // turner.stop();
charwhit 0:0f96a93fbf39 237 }