Blake Babbs / RobotB

one lap kind of works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@2:80a1ed62c307, 2015-05-21 (annotated)

Committer:
nbtavis
Date:
Thu May 21 14:38:47 2015 +0000
Revision:
2:80a1ed62c307
Parent:
1:42bba20ee253
Child:
3:bae8eb81a9d7
my code

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