IESS / Robot

With Libraries

Dependencies:   btbee m3pi_ng mbed FatFileSystem MSCFileSystem

main.cpp@7:7d491b51665e, 2015-05-27 (annotated)

Committer:
charwhit
Date:
Wed May 27 13:36:42 2015 +0000
Revision:
7:7d491b51665e
Parent:
6:99d09e88924b
Child:
8:bd2f012e2f57
This is the best

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 6:99d09e88924b 20 float P_TERM = 1;
charwhit 6:99d09e88924b 21 float I_TERM = 0;
charwhit 6:99d09e88924b 22 float 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 7:7d491b51665e 42 float 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 6:99d09e88924b 49
charwhit 6:99d09e88924b 50
charwhit 6:99d09e88924b 51 wait(8);
charwhit 6:99d09e88924b 52 btbee.printf("Battery: %f\n", robot.battery());
charwhit 6:99d09e88924b 53 //timer.start();
charwhit 5:c38929c0fd95 54
charwhit 5:c38929c0fd95 55
charwhit 5:c38929c0fd95 56 time_wait.start();
charwhit 5:c38929c0fd95 57 while(y) {
charwhit 5:c38929c0fd95 58 time_wait.reset();
charwhit 5:c38929c0fd95 59 //Get raw sensor values
charwhit 5:c38929c0fd95 60 int x [5];
charwhit 5:c38929c0fd95 61 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 62
charwhit 5:c38929c0fd95 63
charwhit 5:c38929c0fd95 64
charwhit 5:c38929c0fd95 65 //Check to make sure battery isn't low
charwhit 5:c38929c0fd95 66 if (robot.battery() < 2.4) {
charwhit 5:c38929c0fd95 67 timer.stop();
charwhit 6:99d09e88924b 68 btbee.printf("Battery too low\n");
charwhit 5:c38929c0fd95 69 break;
charwhit 5:c38929c0fd95 70 }
charwhit 5:c38929c0fd95 71
charwhit 5:c38929c0fd95 72 //else if (m3pi_IN [0] == 0)
charwhit 5:c38929c0fd95 73 //{break;}
charwhit 5:c38929c0fd95 74
charwhit 7:7d491b51665e 75 if( x[0] > 300 && x[2]>300 && x[4]>300) {
charwhit 5:c38929c0fd95 76 if (lap == 0) {
charwhit 5:c38929c0fd95 77 while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 78 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 79 }
charwhit 5:c38929c0fd95 80 timer.start();
charwhit 5:c38929c0fd95 81 lap= lap +1;
charwhit 5:c38929c0fd95 82 }
charwhit 5:c38929c0fd95 83
charwhit 5:c38929c0fd95 84 else if (lap == 2) {
charwhit 6:99d09e88924b 85 robot.stop();
charwhit 5:c38929c0fd95 86 lap_time = timer.read();
charwhit 5:c38929c0fd95 87 total_time += lap_time;
charwhit 5:c38929c0fd95 88 average_time = total_time/lap;
charwhit 5:c38929c0fd95 89 robot.printf("%f",average_time);
charwhit 5:c38929c0fd95 90 if (btbee.writeable()) {
charwhit 5:c38929c0fd95 91 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 5:c38929c0fd95 92 btbee.printf("Avg Lap time: %f\n", average_time);
charwhit 5:c38929c0fd95 93 }
charwhit 6:99d09e88924b 94
charwhit 5:c38929c0fd95 95 while (count < 3){
charwhit 5:c38929c0fd95 96 //btbee.printf("Input parameter\n");
charwhit 5:c38929c0fd95 97 btbee.read_line(arr_read, 30, &chars_read);
charwhit 7:7d491b51665e 98 paramChange[count] = atof(arr_read);
charwhit 6:99d09e88924b 99 //btbee.printf("%d", arr_read);
charwhit 5:c38929c0fd95 100 count++;
charwhit 5:c38929c0fd95 101 }
charwhit 5:c38929c0fd95 102 P_TERM = paramChange[0];
charwhit 5:c38929c0fd95 103 I_TERM = paramChange[1];
charwhit 5:c38929c0fd95 104 D_TERM = paramChange[2];
charwhit 6:99d09e88924b 105 btbee.printf("PTERM %f\n", P_TERM);
charwhit 6:99d09e88924b 106 btbee.printf("ITERM %f\n", I_TERM);
charwhit 6:99d09e88924b 107 btbee.printf("DTERM %f\n", D_TERM);
charwhit 5:c38929c0fd95 108 lap = 0;
charwhit 5:c38929c0fd95 109 total_time = 0;
charwhit 5:c38929c0fd95 110 count = 0;
charwhit 6:99d09e88924b 111 timer.stop();
charwhit 6:99d09e88924b 112 timer.reset();
charwhit 5:c38929c0fd95 113 continue;
charwhit 5:c38929c0fd95 114
charwhit 5:c38929c0fd95 115 } else {
charwhit 5:c38929c0fd95 116 while( x[0]> 300 && x[4] > 300) {
charwhit 5:c38929c0fd95 117 robot.calibrated_sensor(x);
charwhit 5:c38929c0fd95 118 }
charwhit 5:c38929c0fd95 119 lap_time = timer.read();
charwhit 5:c38929c0fd95 120 if (btbee.writeable()) {
charwhit 5:c38929c0fd95 121 btbee.printf("Lap %d time: %f\n", lap, lap_time);
charwhit 5:c38929c0fd95 122 }
charwhit 5:c38929c0fd95 123 total_time += lap_time;
charwhit 5:c38929c0fd95 124 average_time = total_time/lap;
charwhit 5:c38929c0fd95 125 lap = lap +1;
charwhit 5:c38929c0fd95 126 timer.reset();
charwhit 5:c38929c0fd95 127 }
charwhit 5:c38929c0fd95 128 }
charwhit 5:c38929c0fd95 129
nbtavis 2:80a1ed62c307 130
charwhit 5:c38929c0fd95 131 // Get the position of the line.
charwhit 5:c38929c0fd95 132 /* for (int i =0; i < 4; ++i)
charwhit 5:c38929c0fd95 133 current_pos[i] = current_pos[i+1];
charwhit 5:c38929c0fd95 134 current_pos[4] = robot.line_position();
charwhit 5:c38929c0fd95 135 proportional = current_pos[4];
charwhit 5:c38929c0fd95 136
charwhit 5:c38929c0fd95 137 // compute the derivative
charwhit 5:c38929c0fd95 138 derivative = 0;
charwhit 5:c38929c0fd95 139 for (int i =1; i<5;++i) {
charwhit 5:c38929c0fd95 140 if (i ==1)
charwhit 5:c38929c0fd95 141 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 142 else if (i == 2)
charwhit 5:c38929c0fd95 143 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 144 else if (i==3)
charwhit 5:c38929c0fd95 145 derivative += 0*(current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 146 else
charwhit 5:c38929c0fd95 147 derivative += (current_pos[i] - current_pos[i-1]);
charwhit 5:c38929c0fd95 148 }
charwhit 5:c38929c0fd95 149
charwhit 5:c38929c0fd95 150 derivative = derivative; */
charwhit 5:c38929c0fd95 151
charwhit 5:c38929c0fd95 152
charwhit 5:c38929c0fd95 153 current_pos = robot.line_position();
charwhit 5:c38929c0fd95 154 proportional = current_pos;
charwhit 5:c38929c0fd95 155
charwhit 5:c38929c0fd95 156 derivative = current_pos - previous_pos;
charwhit 5:c38929c0fd95 157
charwhit 5:c38929c0fd95 158
charwhit 5:c38929c0fd95 159 //compute the integral
charwhit 5:c38929c0fd95 160 integral =+ proportional;
charwhit 5:c38929c0fd95 161
charwhit 5:c38929c0fd95 162 //remember the last position.
charwhit 5:c38929c0fd95 163 previous_pos = current_pos;
charwhit 5:c38929c0fd95 164
charwhit 5:c38929c0fd95 165 // compute the power
charwhit 5:c38929c0fd95 166 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
charwhit 5:c38929c0fd95 167 //computer new speeds
charwhit 5:c38929c0fd95 168 right = speed+power;
charwhit 5:c38929c0fd95 169 left = speed-power;
charwhit 5:c38929c0fd95 170
charwhit 5:c38929c0fd95 171 //limit checks
charwhit 5:c38929c0fd95 172 if(right<MIN)
charwhit 5:c38929c0fd95 173 right = MIN;
charwhit 5:c38929c0fd95 174 else if (right > MAX)
charwhit 5:c38929c0fd95 175 right = MAX;
charwhit 5:c38929c0fd95 176
charwhit 5:c38929c0fd95 177 if(left<MIN)
charwhit 5:c38929c0fd95 178 left = MIN;
charwhit 5:c38929c0fd95 179 else if (left>MIN)
charwhit 5:c38929c0fd95 180 left = MAX;
charwhit 5:c38929c0fd95 181
charwhit 5:c38929c0fd95 182 //set speed
charwhit 5:c38929c0fd95 183
charwhit 5:c38929c0fd95 184 robot.left_motor(left);
charwhit 5:c38929c0fd95 185 robot.right_motor(right);
charwhit 5:c38929c0fd95 186
charwhit 5:c38929c0fd95 187 wait((5-time_wait.read_ms())/1000);
bbabbs 0:17669460c6b1 188 }
charwhit 5:c38929c0fd95 189
charwhit 5:c38929c0fd95 190
charwhit 5:c38929c0fd95 191
charwhit 5:c38929c0fd95 192 robot.stop();
charwhit 5:c38929c0fd95 193
charwhit 5:c38929c0fd95 194 char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
charwhit 5:c38929c0fd95 195 ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
charwhit 5:c38929c0fd95 196 ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
charwhit 5:c38929c0fd95 197 ,'G','8','E','8','D','8','C','4'
charwhit 5:c38929c0fd95 198 };
nbtavis 2:80a1ed62c307 199 int numb = 59;
charwhit 5:c38929c0fd95 200
nbtavis 2:80a1ed62c307 201 robot.playtune(hail,numb);
charwhit 5:c38929c0fd95 202
charwhit 5:c38929c0fd95 203
charwhit 5:c38929c0fd95 204
charwhit 5:c38929c0fd95 205
charwhit 5:c38929c0fd95 206 }