Blake Babbs / WarehouseBotOther

testing

Dependencies:   btbee m3pi_ng mbed

Fork of WarehouseBot by Blake Babbs

Robot-bae8eb81a9d7/main.cpp@0:0f96a93fbf39, 2015-06-03 (annotated)

Committer:
charwhit
Date:
Wed Jun 03 13:32:30 2015 +0000
Revision:
0:0f96a93fbf39
Child:
1:2db3ccba18c8
Warehouse Bot Code. Trying to get lookup to work

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 0:0f96a93fbf39 42 vector< vector<string> > Lookup(6, vector<string>(6));
charwhit 0:0f96a93fbf39 43
charwhit 0:0f96a93fbf39 44 Lookup[1][4] = "LLS";
charwhit 0:0f96a93fbf39 45
charwhit 0:0f96a93fbf39 46
charwhit 0:0f96a93fbf39 47
charwhit 0:0f96a93fbf39 48
charwhit 0:0f96a93fbf39 49
charwhit 0:0f96a93fbf39 50 time_wait.start();
charwhit 0:0f96a93fbf39 51
charwhit 0:0f96a93fbf39 52 turns = Lookup[0][1];
charwhit 0:0f96a93fbf39 53 while(Run) {
charwhit 0:0f96a93fbf39 54 time_wait.reset();
charwhit 0:0f96a93fbf39 55 //Get raw sensor values
charwhit 0:0f96a93fbf39 56
charwhit 0:0f96a93fbf39 57 robot.calibrated_sensor(x);
charwhit 0:0f96a93fbf39 58
charwhit 0:0f96a93fbf39 59
charwhit 0:0f96a93fbf39 60
charwhit 0:0f96a93fbf39 61 //Check to make sure battery isn't low
charwhit 0:0f96a93fbf39 62 if (robot.battery() < 2.4) {
charwhit 0:0f96a93fbf39 63 timer.stop();
charwhit 0:0f96a93fbf39 64 break;
charwhit 0:0f96a93fbf39 65 }
charwhit 0:0f96a93fbf39 66
charwhit 0:0f96a93fbf39 67
charwhit 0:0f96a93fbf39 68
charwhit 0:0f96a93fbf39 69 if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
charwhit 0:0f96a93fbf39 70
charwhit 0:0f96a93fbf39 71 if(turns.at(direction) == 'L'){
charwhit 0:0f96a93fbf39 72 robot.stop();
charwhit 0:0f96a93fbf39 73 wait(1);
charwhit 0:0f96a93fbf39 74 turnLeft();
charwhit 0:0f96a93fbf39 75 robot.stop();
charwhit 0:0f96a93fbf39 76 goStraight();
charwhit 0:0f96a93fbf39 77 wait(1);
charwhit 0:0f96a93fbf39 78 }
charwhit 0:0f96a93fbf39 79 else if(turns.at(direction) == 'R'){
charwhit 0:0f96a93fbf39 80 robot.stop();
charwhit 0:0f96a93fbf39 81 wait(1);
charwhit 0:0f96a93fbf39 82 turnRight();
charwhit 0:0f96a93fbf39 83 robot.stop();
charwhit 0:0f96a93fbf39 84 goStraight();
charwhit 0:0f96a93fbf39 85 wait(1);
charwhit 0:0f96a93fbf39 86 }
charwhit 0:0f96a93fbf39 87 else{
charwhit 0:0f96a93fbf39 88 robot.stop();
charwhit 0:0f96a93fbf39 89 wait(1);
charwhit 0:0f96a93fbf39 90 goStraight();
charwhit 0:0f96a93fbf39 91 wait(1);
charwhit 0:0f96a93fbf39 92 }
charwhit 0:0f96a93fbf39 93 robot.printf("Size: %d", direction);
charwhit 0:0f96a93fbf39 94 ++direction;
charwhit 0:0f96a93fbf39 95 robot.calibrated_sensor(x);
charwhit 0:0f96a93fbf39 96 }
charwhit 0:0f96a93fbf39 97 else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
charwhit 0:0f96a93fbf39 98 robot.stop();
charwhit 0:0f96a93fbf39 99 break;
charwhit 0:0f96a93fbf39 100 }
charwhit 0:0f96a93fbf39 101 else if (x[0]>300 || x[4]>300 && passed)
charwhit 0:0f96a93fbf39 102 passed = true;
charwhit 0:0f96a93fbf39 103 else
charwhit 0:0f96a93fbf39 104 passed = false;
charwhit 0:0f96a93fbf39 105
charwhit 0:0f96a93fbf39 106
charwhit 0:0f96a93fbf39 107 // Get the position of the line.
charwhit 0:0f96a93fbf39 108 current_pos = robot.line_position();
charwhit 0:0f96a93fbf39 109 proportional = current_pos;
charwhit 0:0f96a93fbf39 110
charwhit 0:0f96a93fbf39 111 derivative = current_pos - previous_pos;
charwhit 0:0f96a93fbf39 112
charwhit 0:0f96a93fbf39 113
charwhit 0:0f96a93fbf39 114 //compute the integral
charwhit 0:0f96a93fbf39 115 integral =+ proportional;
charwhit 0:0f96a93fbf39 116
charwhit 0:0f96a93fbf39 117 //remember the last position.
charwhit 0:0f96a93fbf39 118 previous_pos = current_pos;
charwhit 0:0f96a93fbf39 119
charwhit 0:0f96a93fbf39 120 // compute the power
charwhit 0:0f96a93fbf39 121 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 0:0f96a93fbf39 122 //computer new speeds
charwhit 0:0f96a93fbf39 123 right = speed+power;
charwhit 0:0f96a93fbf39 124 left = speed-power;
charwhit 0:0f96a93fbf39 125
charwhit 0:0f96a93fbf39 126 //limit checks
charwhit 0:0f96a93fbf39 127 if(right<MIN)
charwhit 0:0f96a93fbf39 128 right = MIN;
charwhit 0:0f96a93fbf39 129 else if (right > MAX)
charwhit 0:0f96a93fbf39 130 right = MAX;
charwhit 0:0f96a93fbf39 131
charwhit 0:0f96a93fbf39 132 if(left<MIN)
charwhit 0:0f96a93fbf39 133 left = MIN;
charwhit 0:0f96a93fbf39 134 else if (left>MIN)
charwhit 0:0f96a93fbf39 135 left = MAX;
charwhit 0:0f96a93fbf39 136
charwhit 0:0f96a93fbf39 137 //set speed
charwhit 0:0f96a93fbf39 138
charwhit 0:0f96a93fbf39 139 robot.left_motor(left);
charwhit 0:0f96a93fbf39 140 robot.right_motor(right);
charwhit 0:0f96a93fbf39 141
charwhit 0:0f96a93fbf39 142 wait((5-time_wait.read_ms())/1000);
charwhit 0:0f96a93fbf39 143 }
charwhit 0:0f96a93fbf39 144
charwhit 0:0f96a93fbf39 145
charwhit 0:0f96a93fbf39 146
charwhit 0:0f96a93fbf39 147 robot.stop();
charwhit 0:0f96a93fbf39 148
charwhit 0:0f96a93fbf39 149 char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 0:0f96a93fbf39 150 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 0:0f96a93fbf39 151 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 0:0f96a93fbf39 152 ,'G','8','E','8','D','8','C','4'
charwhit 0:0f96a93fbf39 153 };
charwhit 0:0f96a93fbf39 154 int numb = 59;
charwhit 0:0f96a93fbf39 155
charwhit 0:0f96a93fbf39 156 robot.playtune(hail,numb);
charwhit 0:0f96a93fbf39 157
charwhit 0:0f96a93fbf39 158
charwhit 0:0f96a93fbf39 159 }
charwhit 0:0f96a93fbf39 160
charwhit 0:0f96a93fbf39 161
charwhit 0:0f96a93fbf39 162 void turnRight(){
charwhit 0:0f96a93fbf39 163 Timer turner;
charwhit 0:0f96a93fbf39 164 turner.start();
charwhit 0:0f96a93fbf39 165 while (turner.read_ms() < 105){
charwhit 0:0f96a93fbf39 166 robot.right(.5);
charwhit 0:0f96a93fbf39 167 }
charwhit 0:0f96a93fbf39 168 turner.stop();
charwhit 0:0f96a93fbf39 169 }
charwhit 0:0f96a93fbf39 170
charwhit 0:0f96a93fbf39 171 void turnLeft(){
charwhit 0:0f96a93fbf39 172 Timer turner;
charwhit 0:0f96a93fbf39 173 turner.start();
charwhit 0:0f96a93fbf39 174 while (turner.read_ms() < 105){
charwhit 0:0f96a93fbf39 175 robot.left(.5);
charwhit 0:0f96a93fbf39 176 }
charwhit 0:0f96a93fbf39 177 turner.stop();
charwhit 0:0f96a93fbf39 178 }
charwhit 0:0f96a93fbf39 179
charwhit 0:0f96a93fbf39 180 void goStraight(){
charwhit 0:0f96a93fbf39 181 Timer turner;
charwhit 0:0f96a93fbf39 182 turner.start();
charwhit 0:0f96a93fbf39 183 while (turner.read_ms() < 50){
charwhit 0:0f96a93fbf39 184 robot.forward(.5);
charwhit 0:0f96a93fbf39 185 }
charwhit 0:0f96a93fbf39 186 turner.stop();
charwhit 0:0f96a93fbf39 187 }