Blake Babbs / WarehouseBotOther

testing

Dependencies:   btbee m3pi_ng mbed

Fork of WarehouseBot by Blake Babbs

Robot-bae8eb81a9d7/main.cpp@1:2db3ccba18c8, 2015-06-03 (annotated)

Committer:
charwhit
Date:
Wed Jun 03 14:29:48 2015 +0000
Revision:
1:2db3ccba18c8
Parent:
0:0f96a93fbf39
Child:
2:2d147091491d
I think this 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;
charwhit 0:0f96a93fbf39 9 #define MAX 0.75
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
charwhit 1:2db3ccba18c8 58 turns = blah[(3-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'){
charwhit 1:2db3ccba18c8 79 previous_pos = 0;
charwhit 0:0f96a93fbf39 80 robot.stop();
charwhit 1:2db3ccba18c8 81 //wait(1);
charwhit 0:0f96a93fbf39 82 turnLeft();
charwhit 0:0f96a93fbf39 83 robot.stop();
charwhit 1:2db3ccba18c8 84 //continue;
charwhit 1:2db3ccba18c8 85 /*wait(1);
charwhit 0:0f96a93fbf39 86 goStraight();
charwhit 1:2db3ccba18c8 87 wait(1);*/
charwhit 0:0f96a93fbf39 88 }
charwhit 0:0f96a93fbf39 89 else if(turns.at(direction) == 'R'){
charwhit 1:2db3ccba18c8 90 previous_pos = 0;
charwhit 0:0f96a93fbf39 91 robot.stop();
charwhit 1:2db3ccba18c8 92 //wait(1);
charwhit 0:0f96a93fbf39 93 turnRight();
charwhit 0:0f96a93fbf39 94 robot.stop();
charwhit 1:2db3ccba18c8 95
charwhit 1:2db3ccba18c8 96 /*wait(1);
charwhit 0:0f96a93fbf39 97 goStraight();
charwhit 1:2db3ccba18c8 98 wait(1);*/
charwhit 0:0f96a93fbf39 99 }
charwhit 0:0f96a93fbf39 100 else{
charwhit 0:0f96a93fbf39 101 robot.stop();
charwhit 0:0f96a93fbf39 102 wait(1);
charwhit 0:0f96a93fbf39 103 goStraight();
charwhit 1:2db3ccba18c8 104 //wait(1);
charwhit 0:0f96a93fbf39 105 }
charwhit 1:2db3ccba18c8 106 robot.cls();
charwhit 1:2db3ccba18c8 107 robot.printf("Size %d", direction);
charwhit 0:0f96a93fbf39 108 ++direction;
charwhit 1:2db3ccba18c8 109 passed = true;
charwhit 1:2db3ccba18c8 110 continue;
charwhit 0:0f96a93fbf39 111 }
charwhit 0:0f96a93fbf39 112 else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
charwhit 0:0f96a93fbf39 113 robot.stop();
charwhit 1:2db3ccba18c8 114 robot.cls();
charwhit 1:2db3ccba18c8 115 robot.printf("Try again");
charwhit 0:0f96a93fbf39 116 break;
charwhit 0:0f96a93fbf39 117 }
charwhit 0:0f96a93fbf39 118 else if (x[0]>300 || x[4]>300 && passed)
charwhit 0:0f96a93fbf39 119 passed = true;
charwhit 0:0f96a93fbf39 120 else
charwhit 0:0f96a93fbf39 121 passed = false;
charwhit 0:0f96a93fbf39 122
charwhit 0:0f96a93fbf39 123
charwhit 0:0f96a93fbf39 124 // Get the position of the line.
charwhit 0:0f96a93fbf39 125 current_pos = robot.line_position();
charwhit 0:0f96a93fbf39 126 proportional = current_pos;
charwhit 0:0f96a93fbf39 127
charwhit 0:0f96a93fbf39 128 derivative = current_pos - previous_pos;
charwhit 0:0f96a93fbf39 129
charwhit 0:0f96a93fbf39 130
charwhit 0:0f96a93fbf39 131 //compute the integral
charwhit 0:0f96a93fbf39 132 integral =+ proportional;
charwhit 0:0f96a93fbf39 133
charwhit 0:0f96a93fbf39 134 //remember the last position.
charwhit 0:0f96a93fbf39 135 previous_pos = current_pos;
charwhit 0:0f96a93fbf39 136
charwhit 0:0f96a93fbf39 137 // compute the power
charwhit 0:0f96a93fbf39 138 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 0:0f96a93fbf39 139 //computer new speeds
charwhit 0:0f96a93fbf39 140 right = speed+power;
charwhit 0:0f96a93fbf39 141 left = speed-power;
charwhit 0:0f96a93fbf39 142
charwhit 0:0f96a93fbf39 143 //limit checks
charwhit 0:0f96a93fbf39 144 if(right<MIN)
charwhit 0:0f96a93fbf39 145 right = MIN;
charwhit 0:0f96a93fbf39 146 else if (right > MAX)
charwhit 0:0f96a93fbf39 147 right = MAX;
charwhit 0:0f96a93fbf39 148
charwhit 0:0f96a93fbf39 149 if(left<MIN)
charwhit 0:0f96a93fbf39 150 left = MIN;
charwhit 0:0f96a93fbf39 151 else if (left>MIN)
charwhit 0:0f96a93fbf39 152 left = MAX;
charwhit 0:0f96a93fbf39 153
charwhit 0:0f96a93fbf39 154 //set speed
charwhit 0:0f96a93fbf39 155
charwhit 0:0f96a93fbf39 156 robot.left_motor(left);
charwhit 0:0f96a93fbf39 157 robot.right_motor(right);
charwhit 0:0f96a93fbf39 158
charwhit 0:0f96a93fbf39 159 wait((5-time_wait.read_ms())/1000);
charwhit 0:0f96a93fbf39 160 }
charwhit 0:0f96a93fbf39 161
charwhit 0:0f96a93fbf39 162
charwhit 0:0f96a93fbf39 163
charwhit 0:0f96a93fbf39 164 robot.stop();
charwhit 0:0f96a93fbf39 165
charwhit 1:2db3ccba18c8 166 /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 0:0f96a93fbf39 167 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 0:0f96a93fbf39 168 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 0:0f96a93fbf39 169 ,'G','8','E','8','D','8','C','4'
charwhit 0:0f96a93fbf39 170 };
charwhit 0:0f96a93fbf39 171 int numb = 59;
charwhit 0:0f96a93fbf39 172
charwhit 1:2db3ccba18c8 173 robot.playtune(hail,numb);*/
charwhit 0:0f96a93fbf39 174
charwhit 0:0f96a93fbf39 175
charwhit 0:0f96a93fbf39 176 }
charwhit 0:0f96a93fbf39 177
charwhit 0:0f96a93fbf39 178
charwhit 0:0f96a93fbf39 179 void turnRight(){
charwhit 0:0f96a93fbf39 180 Timer turner;
charwhit 0:0f96a93fbf39 181 turner.start();
charwhit 1:2db3ccba18c8 182 while (turner.read_ms() < 130){
charwhit 0:0f96a93fbf39 183 robot.right(.5);
charwhit 0:0f96a93fbf39 184 }
charwhit 0:0f96a93fbf39 185 turner.stop();
charwhit 0:0f96a93fbf39 186 }
charwhit 0:0f96a93fbf39 187
charwhit 0:0f96a93fbf39 188 void turnLeft(){
charwhit 0:0f96a93fbf39 189 Timer turner;
charwhit 0:0f96a93fbf39 190 turner.start();
charwhit 1:2db3ccba18c8 191 while (turner.read_ms() < 130){
charwhit 0:0f96a93fbf39 192 robot.left(.5);
charwhit 0:0f96a93fbf39 193 }
charwhit 0:0f96a93fbf39 194 turner.stop();
charwhit 0:0f96a93fbf39 195 }
charwhit 0:0f96a93fbf39 196
charwhit 0:0f96a93fbf39 197 void goStraight(){
charwhit 0:0f96a93fbf39 198 Timer turner;
charwhit 0:0f96a93fbf39 199 turner.start();
charwhit 1:2db3ccba18c8 200 while (turner.read_ms() < 25){
charwhit 0:0f96a93fbf39 201 robot.forward(.5);
charwhit 0:0f96a93fbf39 202 }
charwhit 0:0f96a93fbf39 203 turner.stop();
charwhit 0:0f96a93fbf39 204 }