Blake Babbs / RobotB

one lap kind of works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@5:c38929c0fd95, 2015-05-22 (annotated)

Committer:
charwhit
Date:
Fri May 22 15:07:30 2015 +0000
Revision:
5:c38929c0fd95
Parent:
4:acd0f86ed832
Child:
6:99d09e88924b
Bluetooth test working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbabbs 0:17669460c6b1 1 #include "mbed.h"
bbabbs 0:17669460c6b1 2 #include "btbee.h"
bbabbs 0:17669460c6b1 3 #include "m3pi_ng.h"
nbtavis 2:80a1ed62c307 4 m3pi robot;
charwhit 4:acd0f86ed832 5 btbee btbee;
nbtavis 3:bae8eb81a9d7 6 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
nbtavis 2:80a1ed62c307 7 Timer timer;
nbtavis 2:80a1ed62c307 8 Timer time_wait;
nbtavis 2:80a1ed62c307 9 #define MAX 0.95
charwhit 5:c38929c0fd95 10 #define MIN 0
bbabbs 0:17669460c6b1 11
charwhit 4:acd0f86ed832 12 //#define P_TERM 5
charwhit 4:acd0f86ed832 13 //#define I_TERM 0
charwhit 5:c38929c0fd95 14 //#define D_TERM 20
nbtavis 2:80a1ed62c307 15
nbtavis 2:80a1ed62c307 16
bbabbs 0:17669460c6b1 17
charwhit 5:c38929c0fd95 18 int main()
charwhit 5:c38929c0fd95 19 {
charwhit 5:c38929c0fd95 20 int P_TERM = 5;
charwhit 5:c38929c0fd95 21 int I_TERM = 0;
charwhit 5:c38929c0fd95 22 int D_TERM = 20;
charwhit 5:c38929c0fd95 23
charwhit 5:c38929c0fd95 24 btbee.reset();
charwhit 5:c38929c0fd95 25 robot.sensor_auto_calibrate();
charwhit 5:c38929c0fd95 26 wait(2.0);
charwhit 5:c38929c0fd95 27 float right;
charwhit 5:c38929c0fd95 28 float left;
charwhit 5:c38929c0fd95 29 //float current_pos[5];
charwhit 5:c38929c0fd95 30 float current_pos = 0.0;
charwhit 5:c38929c0fd95 31 float previous_pos =0.0;
charwhit 5:c38929c0fd95 32 float derivative, proportional, integral = 0;
charwhit 5:c38929c0fd95 33 float power;
charwhit 5:c38929c0fd95 34 float speed = MAX;
charwhit 5:c38929c0fd95 35
charwhit 5:c38929c0fd95 36 int lap = 0;
charwhit 5:c38929c0fd95 37 float lap_time = 0.0;
charwhit 5:c38929c0fd95 38 float total_time = 0.0;
charwhit 5:c38929c0fd95 39 float average_time = 0.0;
charwhit 5:c38929c0fd95 40 int y =1;
charwhit 5:c38929c0fd95 41 int count = 0;
charwhit 5:c38929c0fd95 42 int paramChange[3];
charwhit 5:c38929c0fd95 43
charwhit 4:acd0f86ed832 44 char arr_read[30]; // this should be long enough to store any reply coming in over bt.
charwhit 4:acd0f86ed832 45 int chars_read;
nbtavis 2:80a1ed62c307 46
charwhit 5:c38929c0fd95 47 /* for (int i = 0; i <5; ++i)
charwhit 5:c38929c0fd95 48 current_pos[i] = 0.0; */
charwhit 5:c38929c0fd95 49 timer.start();
charwhit 5:c38929c0fd95 50
charwhit 5:c38929c0fd95 51
charwhit 5:c38929c0fd95 52 time_wait.start();
charwhit 5:c38929c0fd95 53
charwhit 5:c38929c0fd95 54 wait(8);
charwhit 5:c38929c0fd95 55 while(y) {
charwhit 5:c38929c0fd95 56 time_wait.reset();
charwhit 5:c38929c0fd95 57 //Get raw sensor values
charwhit 5:c38929c0fd95 58 int x [5];
charwhit 5:c38929c0fd95 59 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 60
charwhit 5:c38929c0fd95 61
charwhit 5:c38929c0fd95 62
charwhit 5:c38929c0fd95 63 //Check to make sure battery isn't low
charwhit 5:c38929c0fd95 64 if (robot.battery() < 2.4) {
charwhit 5:c38929c0fd95 65 timer.stop();
charwhit 5:c38929c0fd95 66 break;
charwhit 5:c38929c0fd95 67 }
charwhit 5:c38929c0fd95 68
charwhit 5:c38929c0fd95 69 //else if (m3pi_IN [0] == 0)
charwhit 5:c38929c0fd95 70 //{break;}
charwhit 5:c38929c0fd95 71
charwhit 5:c38929c0fd95 72 else if( x[0] > 300 && x[2]>300 && x[4]>300) {
charwhit 5:c38929c0fd95 73 if (lap == 0) {
charwhit 5:c38929c0fd95 74 while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 75 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 76 }
charwhit 5:c38929c0fd95 77 timer.start();
charwhit 5:c38929c0fd95 78 lap= lap +1;
charwhit 5:c38929c0fd95 79 }
charwhit 5:c38929c0fd95 80
charwhit 5:c38929c0fd95 81 else if (lap == 2) {
charwhit 5:c38929c0fd95 82 lap_time = timer.read();
charwhit 5:c38929c0fd95 83 total_time += lap_time;
charwhit 5:c38929c0fd95 84 average_time = total_time/lap;
charwhit 5:c38929c0fd95 85 robot.printf("%f",average_time);
charwhit 5:c38929c0fd95 86 if (btbee.writeable()) {
charwhit 5:c38929c0fd95 87 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 5:c38929c0fd95 88 btbee.printf("Avg Lap time: %f\n", average_time);
charwhit 5:c38929c0fd95 89 }
charwhit 5:c38929c0fd95 90 robot.stop();
charwhit 5:c38929c0fd95 91 while (count < 3){
charwhit 5:c38929c0fd95 92 //btbee.printf("Input parameter\n");
charwhit 5:c38929c0fd95 93 btbee.read_line(arr_read, 30, &chars_read);
charwhit 5:c38929c0fd95 94 paramChange[count] = atoi(arr_read);
charwhit 5:c38929c0fd95 95 btbee.printf("%d", arr_read);
charwhit 5:c38929c0fd95 96 count++;
charwhit 5:c38929c0fd95 97 }
charwhit 5:c38929c0fd95 98 P_TERM = paramChange[0];
charwhit 5:c38929c0fd95 99 I_TERM = paramChange[1];
charwhit 5:c38929c0fd95 100 D_TERM = paramChange[2];
charwhit 5:c38929c0fd95 101 lap = 0;
charwhit 5:c38929c0fd95 102 total_time = 0;
charwhit 5:c38929c0fd95 103 count = 0;
charwhit 5:c38929c0fd95 104 continue;
charwhit 5:c38929c0fd95 105
charwhit 5:c38929c0fd95 106 } else {
charwhit 5:c38929c0fd95 107 while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 108 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 109 }
charwhit 5:c38929c0fd95 110 lap_time = timer.read();
charwhit 5:c38929c0fd95 111 if (btbee.writeable()) {
charwhit 5:c38929c0fd95 112 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 5:c38929c0fd95 113 }
charwhit 5:c38929c0fd95 114 total_time += lap_time;
charwhit 5:c38929c0fd95 115 average_time = total_time/lap;
charwhit 5:c38929c0fd95 116 lap = lap +1;
charwhit 5:c38929c0fd95 117 timer.reset();
charwhit 5:c38929c0fd95 118 }
charwhit 5:c38929c0fd95 119 }
charwhit 5:c38929c0fd95 120
nbtavis 2:80a1ed62c307 121
charwhit 5:c38929c0fd95 122 // Get the position of the line.
charwhit 5:c38929c0fd95 123 /* for (int i =0; i < 4; ++i)
charwhit 5:c38929c0fd95 124 current_pos[i] = current_pos[i+1];
charwhit 5:c38929c0fd95 125 current_pos[4] = robot.line_position();
charwhit 5:c38929c0fd95 126 proportional = current_pos[4];
charwhit 5:c38929c0fd95 127
charwhit 5:c38929c0fd95 128 // compute the derivative
charwhit 5:c38929c0fd95 129 derivative = 0;
charwhit 5:c38929c0fd95 130 for (int i =1; i<5;++i) {
charwhit 5:c38929c0fd95 131 if (i ==1)
charwhit 5:c38929c0fd95 132 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 133 else if (i == 2)
charwhit 5:c38929c0fd95 134 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 135 else if (i==3)
charwhit 5:c38929c0fd95 136 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 137 else
charwhit 5:c38929c0fd95 138 derivative += (current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 139 }
charwhit 5:c38929c0fd95 140
charwhit 5:c38929c0fd95 141 derivative = derivative; */
charwhit 5:c38929c0fd95 142
charwhit 5:c38929c0fd95 143
charwhit 5:c38929c0fd95 144 current_pos = robot.line_position();
charwhit 5:c38929c0fd95 145 proportional = current_pos;
charwhit 5:c38929c0fd95 146
charwhit 5:c38929c0fd95 147 derivative = current_pos - previous_pos;
charwhit 5:c38929c0fd95 148
charwhit 5:c38929c0fd95 149
charwhit 5:c38929c0fd95 150 //compute the integral
charwhit 5:c38929c0fd95 151 integral =+ proportional;
charwhit 5:c38929c0fd95 152
charwhit 5:c38929c0fd95 153 //remember the last position.
charwhit 5:c38929c0fd95 154 previous_pos = current_pos;
charwhit 5:c38929c0fd95 155
charwhit 5:c38929c0fd95 156 // compute the power
charwhit 5:c38929c0fd95 157 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 5:c38929c0fd95 158 //computer new speeds
charwhit 5:c38929c0fd95 159 right = speed+power;
charwhit 5:c38929c0fd95 160 left = speed-power;
charwhit 5:c38929c0fd95 161
charwhit 5:c38929c0fd95 162 //limit checks
charwhit 5:c38929c0fd95 163 if(right<MIN)
charwhit 5:c38929c0fd95 164 right = MIN;
charwhit 5:c38929c0fd95 165 else if (right > MAX)
charwhit 5:c38929c0fd95 166 right = MAX;
charwhit 5:c38929c0fd95 167
charwhit 5:c38929c0fd95 168 if(left<MIN)
charwhit 5:c38929c0fd95 169 left = MIN;
charwhit 5:c38929c0fd95 170 else if (left>MIN)
charwhit 5:c38929c0fd95 171 left = MAX;
charwhit 5:c38929c0fd95 172
charwhit 5:c38929c0fd95 173 //set speed
charwhit 5:c38929c0fd95 174
charwhit 5:c38929c0fd95 175 robot.left_motor(left);
charwhit 5:c38929c0fd95 176 robot.right_motor(right);
charwhit 5:c38929c0fd95 177
charwhit 5:c38929c0fd95 178 wait((5-time_wait.read_ms())/1000);
bbabbs 0:17669460c6b1 179 }
charwhit 5:c38929c0fd95 180
charwhit 5:c38929c0fd95 181
charwhit 5:c38929c0fd95 182
charwhit 5:c38929c0fd95 183 robot.stop();
charwhit 5:c38929c0fd95 184
charwhit 5:c38929c0fd95 185 char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 5:c38929c0fd95 186 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 5:c38929c0fd95 187 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 5:c38929c0fd95 188 ,'G','8','E','8','D','8','C','4'
charwhit 5:c38929c0fd95 189 };
nbtavis 2:80a1ed62c307 190 int numb = 59;
charwhit 5:c38929c0fd95 191
nbtavis 2:80a1ed62c307 192 robot.playtune(hail,numb);
charwhit 5:c38929c0fd95 193
charwhit 5:c38929c0fd95 194
charwhit 5:c38929c0fd95 195
charwhit 5:c38929c0fd95 196
charwhit 5:c38929c0fd95 197 }