tetsuya yoshikawa / Mbed 2 deprecated Line_Trace_No_collision_Light_control_longer_dis

test

Dependencies:   S11059 VL6180X m3pi mbed

Fork of tc_agent by Ichiro Maruta

main.cpp@3:f1ddc26da601, 2017-02-08 (annotated)

Committer:
tennisbaca
Date:
Wed Feb 08 15:53:58 2017 +0000
Revision:
3:f1ddc26da601
Parent:
2:3ebca956fd36
Child:
4:0e6997707f43
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maruta 2:3ebca956fd36 1 #include "VL6180X.h"
sburg 0:2508f38e90fe 2 #include "mbed.h"
tennisbaca 1:6340d62d759f 3 #include "m3pi.h"
maruta 2:3ebca956fd36 4 #include "S11059.h"
sburg 0:2508f38e90fe 5
maruta 2:3ebca956fd36 6 VL6180x rf(p28, p27); //I2C sda and scl
sburg 0:2508f38e90fe 7 Serial pc(USBTX, USBRX); //USB serial
maruta 2:3ebca956fd36 8 S11059 col(p28,p27);
tennisbaca 1:6340d62d759f 9 m3pi m3pi;
tennisbaca 1:6340d62d759f 10 Timer t;
tennisbaca 3:f1ddc26da601 11 // Minimum and maximum motor speeds
tennisbaca 3:f1ddc26da601 12 #define MAX 1
tennisbaca 3:f1ddc26da601 13 #define MIN 0
tennisbaca 3:f1ddc26da601 14
tennisbaca 3:f1ddc26da601 15 // PID terms
tennisbaca 3:f1ddc26da601 16 #define P_TERM 1
tennisbaca 3:f1ddc26da601 17 #define I_TERM 0
tennisbaca 3:f1ddc26da601 18 #define D_TERM 20
tennisbaca 3:f1ddc26da601 19
tennisbaca 3:f1ddc26da601 20 // PID terms for collision
tennisbaca 3:f1ddc26da601 21 #define P_TERM_COL 1
tennisbaca 3:f1ddc26da601 22 #define I_TERM_COL 0
tennisbaca 3:f1ddc26da601 23 #define D_TERM_COL 20
tennisbaca 3:f1ddc26da601 24
sburg 0:2508f38e90fe 25
sburg 0:2508f38e90fe 26 int main() {
tennisbaca 1:6340d62d759f 27 m3pi.cls();
tennisbaca 3:f1ddc26da601 28 //t_for_music.start();
tennisbaca 1:6340d62d759f 29 m3pi.locate(0,0);
tennisbaca 3:f1ddc26da601 30 m3pi.printf("Battery");
tennisbaca 1:6340d62d759f 31 m3pi.locate(0,1);
tennisbaca 3:f1ddc26da601 32 float battery = m3pi.battery();
tennisbaca 3:f1ddc26da601 33 m3pi.printf("%.0fmV",battery*1000);
tennisbaca 3:f1ddc26da601 34 wait(3);
tennisbaca 3:f1ddc26da601 35
tennisbaca 3:f1ddc26da601 36
tennisbaca 3:f1ddc26da601 37 // distance sensor
maruta 2:3ebca956fd36 38 int reading;
tennisbaca 1:6340d62d759f 39 float time[2];
maruta 2:3ebca956fd36 40 m3pi.cls();
maruta 2:3ebca956fd36 41 rf.VL6180xInit();
maruta 2:3ebca956fd36 42 rf.VL6180xDefautSettings();
tennisbaca 3:f1ddc26da601 43 int check = 1;
tennisbaca 3:f1ddc26da601 44 int Sensor_num = 0;
tennisbaca 3:f1ddc26da601 45 int reading_history[3];
tennisbaca 3:f1ddc26da601 46 int i;
tennisbaca 3:f1ddc26da601 47 for(i = 0;i<3;i++){
tennisbaca 3:f1ddc26da601 48 reading_history[i] = 255;
tennisbaca 3:f1ddc26da601 49 }
tennisbaca 3:f1ddc26da601 50 float ave_reading;
tennisbaca 3:f1ddc26da601 51
tennisbaca 1:6340d62d759f 52
tennisbaca 3:f1ddc26da601 53 rf.triggerDistance();
tennisbaca 3:f1ddc26da601 54
tennisbaca 3:f1ddc26da601 55
tennisbaca 3:f1ddc26da601 56 m3pi.sensor_auto_calibrate();
tennisbaca 3:f1ddc26da601 57 // variables for PID
tennisbaca 3:f1ddc26da601 58 float right;
tennisbaca 3:f1ddc26da601 59 float left;
tennisbaca 3:f1ddc26da601 60 float current_pos_of_line = 0.0;
tennisbaca 3:f1ddc26da601 61 float previous_pos_of_line = 0.0;
tennisbaca 3:f1ddc26da601 62 float derivative,proportional,integral = 0;
tennisbaca 3:f1ddc26da601 63 float power;
tennisbaca 3:f1ddc26da601 64 float power_collision;
tennisbaca 3:f1ddc26da601 65 float speed = MAX;
tennisbaca 3:f1ddc26da601 66
tennisbaca 3:f1ddc26da601 67
tennisbaca 3:f1ddc26da601 68 //for color sensor
tennisbaca 3:f1ddc26da601 69 int bl=0;
tennisbaca 3:f1ddc26da601 70 int gr=0;
tennisbaca 3:f1ddc26da601 71 int re=0;
tennisbaca 3:f1ddc26da601 72
tennisbaca 3:f1ddc26da601 73 t.start();
tennisbaca 3:f1ddc26da601 74
sburg 0:2508f38e90fe 75 while(1) {
tennisbaca 3:f1ddc26da601 76 Sensor_num = Sensor_num + 1;
tennisbaca 1:6340d62d759f 77 m3pi.cls();
tennisbaca 3:f1ddc26da601 78 //t.start();
tennisbaca 3:f1ddc26da601 79 //time[0] = t.read();
tennisbaca 3:f1ddc26da601 80 col.update();
tennisbaca 3:f1ddc26da601 81 bl=col.b;
tennisbaca 3:f1ddc26da601 82 gr=col.g;
tennisbaca 3:f1ddc26da601 83 re=col.r;
tennisbaca 3:f1ddc26da601 84
tennisbaca 3:f1ddc26da601 85 // Get the position of the line.
tennisbaca 3:f1ddc26da601 86 current_pos_of_line = m3pi.line_position();
tennisbaca 3:f1ddc26da601 87 proportional = current_pos_of_line;
tennisbaca 3:f1ddc26da601 88
tennisbaca 3:f1ddc26da601 89 // Compute the derivative
tennisbaca 3:f1ddc26da601 90 derivative = current_pos_of_line - previous_pos_of_line;
tennisbaca 3:f1ddc26da601 91
tennisbaca 3:f1ddc26da601 92 // Compute the integral
tennisbaca 3:f1ddc26da601 93 integral += proportional;
tennisbaca 3:f1ddc26da601 94
tennisbaca 3:f1ddc26da601 95 // Remember the last position.
tennisbaca 3:f1ddc26da601 96 previous_pos_of_line = current_pos_of_line;
tennisbaca 3:f1ddc26da601 97
tennisbaca 3:f1ddc26da601 98 // Compute the power
tennisbaca 3:f1ddc26da601 99 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
tennisbaca 3:f1ddc26da601 100 power_collision = (proportional * (P_TERM_COL) ) + (integral*(I_TERM_COL)) + (derivative*(D_TERM_COL)) ;
tennisbaca 3:f1ddc26da601 101
tennisbaca 3:f1ddc26da601 102 // Compute new speeds if there is nothing ahead
tennisbaca 3:f1ddc26da601 103 if(ave_reading >= 190 && (gr+re) <= 7000){
tennisbaca 3:f1ddc26da601 104 right = speed+power;
tennisbaca 3:f1ddc26da601 105 left = speed-power;
tennisbaca 3:f1ddc26da601 106
tennisbaca 3:f1ddc26da601 107 // limit checks
tennisbaca 3:f1ddc26da601 108 if (right < MIN)
tennisbaca 3:f1ddc26da601 109 right = MIN;
tennisbaca 3:f1ddc26da601 110 else if (right > MAX)
tennisbaca 3:f1ddc26da601 111 right = MAX;
tennisbaca 3:f1ddc26da601 112
tennisbaca 3:f1ddc26da601 113 if (left < MIN)
tennisbaca 3:f1ddc26da601 114 left = MIN;
tennisbaca 3:f1ddc26da601 115 else if (left > MAX)
tennisbaca 3:f1ddc26da601 116 left = MAX;
tennisbaca 3:f1ddc26da601 117 }
tennisbaca 3:f1ddc26da601 118
tennisbaca 3:f1ddc26da601 119 // get distance once every three times because of the processing speed
tennisbaca 3:f1ddc26da601 120 if((Sensor_num %3)== 1){
tennisbaca 3:f1ddc26da601 121 reading = rf.pollDistance();
tennisbaca 3:f1ddc26da601 122 rf.triggerDistance();
tennisbaca 3:f1ddc26da601 123 if(reading < 90 && reading > 0.1){
tennisbaca 3:f1ddc26da601 124 /*m3pi.locate(0,0);
tennisbaca 3:f1ddc26da601 125 m3pi.printf("%dmm",reading);
tennisbaca 3:f1ddc26da601 126 wait(10);*/
tennisbaca 3:f1ddc26da601 127 reading = 90;
tennisbaca 3:f1ddc26da601 128 }else if(reading < 0.1){
tennisbaca 3:f1ddc26da601 129 reading = 255;
tennisbaca 3:f1ddc26da601 130 }
tennisbaca 3:f1ddc26da601 131 reading_history[0] = reading_history[1];
tennisbaca 3:f1ddc26da601 132 reading_history[1] = reading_history[2];
tennisbaca 3:f1ddc26da601 133 reading_history[2] = reading;
tennisbaca 3:f1ddc26da601 134 ave_reading = float(reading_history[0]+reading_history[1]+reading_history[2])/3;
tennisbaca 3:f1ddc26da601 135 }
tennisbaca 3:f1ddc26da601 136
tennisbaca 3:f1ddc26da601 137 // if distance is too close, change the speed
tennisbaca 3:f1ddc26da601 138 if(ave_reading < 190){
tennisbaca 3:f1ddc26da601 139 right = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
tennisbaca 3:f1ddc26da601 140 left = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
tennisbaca 3:f1ddc26da601 141 // limit checks
tennisbaca 3:f1ddc26da601 142 if (right < MIN)
tennisbaca 3:f1ddc26da601 143 right = MIN;
tennisbaca 3:f1ddc26da601 144 else if (right > MAX)
tennisbaca 3:f1ddc26da601 145 right = MAX;
tennisbaca 3:f1ddc26da601 146
tennisbaca 3:f1ddc26da601 147 if (left < MIN)
tennisbaca 3:f1ddc26da601 148 left = MIN;
tennisbaca 3:f1ddc26da601 149 else if (left > MAX)
tennisbaca 3:f1ddc26da601 150 left = MAX;
tennisbaca 3:f1ddc26da601 151 }
tennisbaca 3:f1ddc26da601 152
tennisbaca 3:f1ddc26da601 153 //if light is on its left side, use light to control
tennisbaca 3:f1ddc26da601 154
tennisbaca 3:f1ddc26da601 155 if((gr+re) > 7000 && ave_reading > 190){
tennisbaca 3:f1ddc26da601 156 // if(Sensor_num%3 == 2){
tennisbaca 3:f1ddc26da601 157 double light_position = (((double)gr)/((double) (re+gr)));
tennisbaca 3:f1ddc26da601 158 double light_speed = light_position*(10.0/7.0)-(2.0/7.0);
tennisbaca 3:f1ddc26da601 159 right = light_speed + light_speed*power_collision;
tennisbaca 3:f1ddc26da601 160 left = light_speed - light_speed*power_collision;
tennisbaca 3:f1ddc26da601 161
tennisbaca 3:f1ddc26da601 162 // limit checks
tennisbaca 3:f1ddc26da601 163 if (right < MIN)
tennisbaca 3:f1ddc26da601 164 right = MIN;
tennisbaca 3:f1ddc26da601 165 else if (right > MAX)
tennisbaca 3:f1ddc26da601 166 right = MAX;
tennisbaca 3:f1ddc26da601 167
tennisbaca 3:f1ddc26da601 168 if (left < MIN)
tennisbaca 3:f1ddc26da601 169 left = MIN;
tennisbaca 3:f1ddc26da601 170 else if (left > MAX)
tennisbaca 3:f1ddc26da601 171 left = MAX;
tennisbaca 3:f1ddc26da601 172 // }
tennisbaca 3:f1ddc26da601 173 }
tennisbaca 3:f1ddc26da601 174
tennisbaca 3:f1ddc26da601 175
tennisbaca 3:f1ddc26da601 176 m3pi.left_motor(left);
tennisbaca 3:f1ddc26da601 177 m3pi.right_motor(right);
tennisbaca 3:f1ddc26da601 178 //wait_ms(1);
tennisbaca 3:f1ddc26da601 179 while(t.read() - time[0] < 0.005){
tennisbaca 3:f1ddc26da601 180 check = 0;
tennisbaca 3:f1ddc26da601 181 }
tennisbaca 3:f1ddc26da601 182 if(check == 1){
tennisbaca 3:f1ddc26da601 183 m3pi.locate(0,0);
tennisbaca 3:f1ddc26da601 184 m3pi.printf("%dmm",reading);
tennisbaca 3:f1ddc26da601 185 wait(1);
tennisbaca 3:f1ddc26da601 186 m3pi.stop();
tennisbaca 3:f1ddc26da601 187 }
tennisbaca 3:f1ddc26da601 188 check = 1;
tennisbaca 3:f1ddc26da601 189 t.stop();
tennisbaca 1:6340d62d759f 190 t.start();
tennisbaca 1:6340d62d759f 191 time[0] = t.read();
sburg 0:2508f38e90fe 192 }
sburg 0:2508f38e90fe 193 }