Blake Babbs / WarehouseBotOther

testing

Dependencies:   btbee m3pi_ng mbed

Fork of WarehouseBot by Blake Babbs

Robot-bae8eb81a9d7/main.cpp@2:2d147091491d, 2015-06-04 (annotated)

Committer:
bbabbs
Date:
Thu Jun 04 11:57:24 2015 +0000
Revision:
2:2d147091491d
Parent:
1:2db3ccba18c8
Child:
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
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;
charwhit 1:2db3ccba18c8 42 string blah[36] = {"", "LRRL", "SSRL", "RLSRL", "RLLLRSRLL", "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 2:2d147091491d 58 turns = blah[(4-1)*6+(2-1)];
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
charwhit 0:0f96a93fbf39 65
charwhit 0:0f96a93fbf39 66
charwhit 0:0f96a93fbf39 67 //Check to make sure battery isn't low
charwhit 0:0f96a93fbf39 68 if (robot.battery() < 2.4) {
charwhit 0:0f96a93fbf39 69 timer.stop();
charwhit 0:0f96a93fbf39 70 break;
charwhit 0:0f96a93fbf39 71 }
charwhit 0:0f96a93fbf39 72
charwhit 0:0f96a93fbf39 73
charwhit 1:2db3ccba18c8 74
charwhit 0:0f96a93fbf39 75 if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
charwhit 0:0f96a93fbf39 76
charwhit 1:2db3ccba18c8 77
charwhit 0:0f96a93fbf39 78 if(turns.at(direction) == 'L'){
bbabbs 2:2d147091491d 79 while(x[0] > 300){
bbabbs 2:2d147091491d 80 goStraight();
bbabbs 2:2d147091491d 81 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 82 }
charwhit 1:2db3ccba18c8 83 previous_pos = 0;
charwhit 0:0f96a93fbf39 84 robot.stop();
charwhit 1:2db3ccba18c8 85 //wait(1);
bbabbs 2:2d147091491d 86 while(/*x[0] > 300 &&*/ x[2] > 300){
bbabbs 2:2d147091491d 87 turnLeft();
bbabbs 2:2d147091491d 88 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 89 }
bbabbs 2:2d147091491d 90
bbabbs 2:2d147091491d 91 while((x[0] > 300) || (x[2] < 300)){
bbabbs 2:2d147091491d 92 turnLeft();
bbabbs 2:2d147091491d 93 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 94 }
charwhit 0:0f96a93fbf39 95 robot.stop();
charwhit 1:2db3ccba18c8 96 //continue;
charwhit 1:2db3ccba18c8 97 /*wait(1);
charwhit 0:0f96a93fbf39 98 goStraight();
charwhit 1:2db3ccba18c8 99 wait(1);*/
charwhit 0:0f96a93fbf39 100 }
charwhit 0:0f96a93fbf39 101 else if(turns.at(direction) == 'R'){
charwhit 1:2db3ccba18c8 102 previous_pos = 0;
charwhit 0:0f96a93fbf39 103 robot.stop();
charwhit 1:2db3ccba18c8 104 //wait(1);
bbabbs 2:2d147091491d 105 while(x[4] > 300 && x[2] > 300){
bbabbs 2:2d147091491d 106 turnRight();
bbabbs 2:2d147091491d 107 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 108 }
bbabbs 2:2d147091491d 109
bbabbs 2:2d147091491d 110 while((x[4] > 300) || (x[2] < 300)){
bbabbs 2:2d147091491d 111 turnRight();
bbabbs 2:2d147091491d 112 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 113 }
charwhit 0:0f96a93fbf39 114 robot.stop();
charwhit 1:2db3ccba18c8 115
charwhit 1:2db3ccba18c8 116 /*wait(1);
charwhit 0:0f96a93fbf39 117 goStraight();
charwhit 1:2db3ccba18c8 118 wait(1);*/
charwhit 0:0f96a93fbf39 119 }
charwhit 0:0f96a93fbf39 120 else{
charwhit 0:0f96a93fbf39 121 robot.stop();
bbabbs 2:2d147091491d 122 //wait(1);
bbabbs 2:2d147091491d 123 while(x[0] > 300 || x[4] > 300){
bbabbs 2:2d147091491d 124 goStraight();
bbabbs 2:2d147091491d 125 robot.calibrated_sensor(x);
bbabbs 2:2d147091491d 126 }
charwhit 1:2db3ccba18c8 127 //wait(1);
charwhit 0:0f96a93fbf39 128 }
charwhit 1:2db3ccba18c8 129 robot.cls();
charwhit 1:2db3ccba18c8 130 robot.printf("Size %d", direction);
charwhit 0:0f96a93fbf39 131 ++direction;
charwhit 1:2db3ccba18c8 132 passed = true;
charwhit 1:2db3ccba18c8 133 continue;
charwhit 0:0f96a93fbf39 134 }
charwhit 0:0f96a93fbf39 135 else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
charwhit 0:0f96a93fbf39 136 robot.stop();
charwhit 1:2db3ccba18c8 137 robot.cls();
charwhit 1:2db3ccba18c8 138 robot.printf("Try again");
charwhit 0:0f96a93fbf39 139 break;
charwhit 0:0f96a93fbf39 140 }
charwhit 0:0f96a93fbf39 141 else if (x[0]>300 || x[4]>300 && passed)
charwhit 0:0f96a93fbf39 142 passed = true;
charwhit 0:0f96a93fbf39 143 else
charwhit 0:0f96a93fbf39 144 passed = false;
charwhit 0:0f96a93fbf39 145
charwhit 0:0f96a93fbf39 146
charwhit 0:0f96a93fbf39 147 // Get the position of the line.
charwhit 0:0f96a93fbf39 148 current_pos = robot.line_position();
charwhit 0:0f96a93fbf39 149 proportional = current_pos;
charwhit 0:0f96a93fbf39 150
charwhit 0:0f96a93fbf39 151 derivative = current_pos - previous_pos;
charwhit 0:0f96a93fbf39 152
charwhit 0:0f96a93fbf39 153
charwhit 0:0f96a93fbf39 154 //compute the integral
charwhit 0:0f96a93fbf39 155 integral =+ proportional;
charwhit 0:0f96a93fbf39 156
charwhit 0:0f96a93fbf39 157 //remember the last position.
charwhit 0:0f96a93fbf39 158 previous_pos = current_pos;
charwhit 0:0f96a93fbf39 159
charwhit 0:0f96a93fbf39 160 // compute the power
charwhit 0:0f96a93fbf39 161 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 0:0f96a93fbf39 162 //computer new speeds
charwhit 0:0f96a93fbf39 163 right = speed+power;
charwhit 0:0f96a93fbf39 164 left = speed-power;
charwhit 0:0f96a93fbf39 165
charwhit 0:0f96a93fbf39 166 //limit checks
charwhit 0:0f96a93fbf39 167 if(right<MIN)
charwhit 0:0f96a93fbf39 168 right = MIN;
charwhit 0:0f96a93fbf39 169 else if (right > MAX)
charwhit 0:0f96a93fbf39 170 right = MAX;
charwhit 0:0f96a93fbf39 171
charwhit 0:0f96a93fbf39 172 if(left<MIN)
charwhit 0:0f96a93fbf39 173 left = MIN;
charwhit 0:0f96a93fbf39 174 else if (left>MIN)
charwhit 0:0f96a93fbf39 175 left = MAX;
charwhit 0:0f96a93fbf39 176
charwhit 0:0f96a93fbf39 177 //set speed
charwhit 0:0f96a93fbf39 178
charwhit 0:0f96a93fbf39 179 robot.left_motor(left);
charwhit 0:0f96a93fbf39 180 robot.right_motor(right);
charwhit 0:0f96a93fbf39 181
charwhit 0:0f96a93fbf39 182 wait((5-time_wait.read_ms())/1000);
charwhit 0:0f96a93fbf39 183 }
charwhit 0:0f96a93fbf39 184
charwhit 0:0f96a93fbf39 185
charwhit 0:0f96a93fbf39 186
charwhit 0:0f96a93fbf39 187 robot.stop();
charwhit 0:0f96a93fbf39 188
charwhit 1:2db3ccba18c8 189 /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 0:0f96a93fbf39 190 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 0:0f96a93fbf39 191 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 0:0f96a93fbf39 192 ,'G','8','E','8','D','8','C','4'
charwhit 0:0f96a93fbf39 193 };
charwhit 0:0f96a93fbf39 194 int numb = 59;
charwhit 0:0f96a93fbf39 195
charwhit 1:2db3ccba18c8 196 robot.playtune(hail,numb);*/
charwhit 0:0f96a93fbf39 197
charwhit 0:0f96a93fbf39 198
charwhit 0:0f96a93fbf39 199 }
charwhit 0:0f96a93fbf39 200
charwhit 0:0f96a93fbf39 201
charwhit 0:0f96a93fbf39 202 void turnRight(){
bbabbs 2:2d147091491d 203 //Timer turner;
bbabbs 2:2d147091491d 204 //turner.start();
bbabbs 2:2d147091491d 205 //while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 206 robot.right(.5);
bbabbs 2:2d147091491d 207 //}
bbabbs 2:2d147091491d 208 //turner.stop();
charwhit 0:0f96a93fbf39 209 }
charwhit 0:0f96a93fbf39 210
charwhit 0:0f96a93fbf39 211 void turnLeft(){
bbabbs 2:2d147091491d 212 // Timer turner;
bbabbs 2:2d147091491d 213 // turner.start();
bbabbs 2:2d147091491d 214 // while (turner.read_ms() < 10){
charwhit 0:0f96a93fbf39 215 robot.left(.5);
bbabbs 2:2d147091491d 216 // }
bbabbs 2:2d147091491d 217 //turner.stop();
charwhit 0:0f96a93fbf39 218 }
charwhit 0:0f96a93fbf39 219
charwhit 0:0f96a93fbf39 220 void goStraight(){
bbabbs 2:2d147091491d 221 // Timer turner;
bbabbs 2:2d147091491d 222 // turner.start();
bbabbs 2:2d147091491d 223 // while (turner.read_ms() < 5){
charwhit 0:0f96a93fbf39 224 robot.forward(.5);
bbabbs 2:2d147091491d 225 // }
bbabbs 2:2d147091491d 226 // turner.stop();
charwhit 0:0f96a93fbf39 227 }