Blake Babbs / RobotB

one lap kind of works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@4:acd0f86ed832, 2015-05-22 (annotated)

Committer:
charwhit
Date:
Fri May 22 14:21:22 2015 +0000
Revision:
4:acd0f86ed832
Parent:
3:bae8eb81a9d7
Child:
5:c38929c0fd95
Before speed testing

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