Blake Babbs / RobotB

one lap kind of works

Dependencies:   FatFileSystem MSCFileSystem btbee m3pi_ng mbed

Fork of Robot by IESS

main.cpp@3:bae8eb81a9d7, 2015-05-22 (annotated)

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