Mechatro5 / Mbed OS LINE_TRACE_CAR

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BMX055 Motor Way

main.cpp@3:efc081576a33, 2019-12-12 (annotated)

Committer:
yasunorihirakawa
Date:
Thu Dec 12 07:27:36 2019 +0000
Revision:
3:efc081576a33
Parent:
2:09ea66e396c1
Child:
4:5588f67b8c48
5sensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arima 0:532ef32974cf 1 #include "mbed.h"
yasunorihirakawa 1:bb85c9fe1ba3 2 #include "motor.h"
yasunorihirakawa 1:bb85c9fe1ba3 3
yasunorihirakawa 1:bb85c9fe1ba3 4 DigitalOut motor_mode(D9);
yasunorihirakawa 3:efc081576a33 5 DigitalOut LED(D10);
yasunorihirakawa 1:bb85c9fe1ba3 6 Motor motorL(D5, D6);
yasunorihirakawa 1:bb85c9fe1ba3 7 Motor motorR(D3, D4);
yasunorihirakawa 1:bb85c9fe1ba3 8 InterruptIn pg1(D12);
yasunorihirakawa 1:bb85c9fe1ba3 9 InterruptIn pg2(D11);
yasunorihirakawa 3:efc081576a33 10 AnalogIn reflectorFL(A6);
yasunorihirakawa 3:efc081576a33 11 AnalogIn reflectorFM(A3);
yasunorihirakawa 3:efc081576a33 12 AnalogIn reflectorFR(A2);
yasunorihirakawa 3:efc081576a33 13 AnalogIn reflectorBL(A1);
yasunorihirakawa 3:efc081576a33 14 AnalogIn reflectorBR(A0);
yasunorihirakawa 3:efc081576a33 15 //AnalogIn reflectorF(A2);
yasunorihirakawa 3:efc081576a33 16 //AnalogIn reflectorL(A1);
yasunorihirakawa 3:efc081576a33 17 //AnalogIn reflectorR(A0);
yasunorihirakawa 1:bb85c9fe1ba3 18
yasunorihirakawa 1:bb85c9fe1ba3 19 Thread thread_trace;
yasunorihirakawa 1:bb85c9fe1ba3 20 Thread thread_motor;
Arima 0:532ef32974cf 21
yasunorihirakawa 1:bb85c9fe1ba3 22 const int THREAD_FLAG_TRACE(1);
yasunorihirakawa 1:bb85c9fe1ba3 23 const int THREAD_FLAG_MOTOR(2);
yasunorihirakawa 1:bb85c9fe1ba3 24 const float INTVAL_REFLECTOR (0.01);
yasunorihirakawa 1:bb85c9fe1ba3 25 const float INTVAL_MOTOR (0.25);
yasunorihirakawa 1:bb85c9fe1ba3 26
yasunorihirakawa 1:bb85c9fe1ba3 27 float dist = 0.0;
yasunorihirakawa 3:efc081576a33 28 float fast_rpm = 0;
yasunorihirakawa 2:09ea66e396c1 29 float standard_rpm = 0;
yasunorihirakawa 3:efc081576a33 30 float slow_rpm = 0;
yasunorihirakawa 1:bb85c9fe1ba3 31
yasunorihirakawa 1:bb85c9fe1ba3 32 void count_pg1()
yasunorihirakawa 1:bb85c9fe1ba3 33 {
yasunorihirakawa 1:bb85c9fe1ba3 34 motorL.count();
yasunorihirakawa 1:bb85c9fe1ba3 35 }
yasunorihirakawa 1:bb85c9fe1ba3 36
yasunorihirakawa 1:bb85c9fe1ba3 37 void count_pg2()
yasunorihirakawa 1:bb85c9fe1ba3 38 {
yasunorihirakawa 1:bb85c9fe1ba3 39 motorR.count();
yasunorihirakawa 3:efc081576a33 40 }
yasunorihirakawa 1:bb85c9fe1ba3 41
yasunorihirakawa 1:bb85c9fe1ba3 42 void line_trace()
Arima 0:532ef32974cf 43 {
yasunorihirakawa 3:efc081576a33 44 //const float fast_rpm = 0;
yasunorihirakawa 3:efc081576a33 45 //const float standard_rpm = 0;
yasunorihirakawa 3:efc081576a33 46 //const float slow_rpm = 0;
yasunorihirakawa 3:efc081576a33 47 static float pre = 0.0;
yasunorihirakawa 3:efc081576a33 48 const float KP = 700;//850;
yasunorihirakawa 3:efc081576a33 49 const float KD = 500;
yasunorihirakawa 1:bb85c9fe1ba3 50 const int L = 0;
yasunorihirakawa 1:bb85c9fe1ba3 51 const int R = 1;
yasunorihirakawa 1:bb85c9fe1ba3 52
yasunorihirakawa 1:bb85c9fe1ba3 53 int flag;
yasunorihirakawa 1:bb85c9fe1ba3 54
yasunorihirakawa 1:bb85c9fe1ba3 55 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 56 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 57
yasunorihirakawa 1:bb85c9fe1ba3 58 while(1)
yasunorihirakawa 1:bb85c9fe1ba3 59 {
yasunorihirakawa 3:efc081576a33 60 if( reflectorFM<=0.4 && reflectorFL<=0.4 && reflectorFR<=0.4 )
yasunorihirakawa 1:bb85c9fe1ba3 61 {
yasunorihirakawa 1:bb85c9fe1ba3 62 if( flag == L )
yasunorihirakawa 1:bb85c9fe1ba3 63 {
yasunorihirakawa 3:efc081576a33 64 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 65 motorL.Set_target(0);
yasunorihirakawa 1:bb85c9fe1ba3 66 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 67 motorR.Set_target(standard_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 68 //printf("turnL\n");
yasunorihirakawa 1:bb85c9fe1ba3 69 }
yasunorihirakawa 1:bb85c9fe1ba3 70 else
yasunorihirakawa 1:bb85c9fe1ba3 71 {
yasunorihirakawa 1:bb85c9fe1ba3 72 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 73 motorL.Set_target(standard_rpm);
yasunorihirakawa 3:efc081576a33 74 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 75 motorR.Set_target(0);
yasunorihirakawa 3:efc081576a33 76 //printf("turnR\n");
yasunorihirakawa 3:efc081576a33 77 }
yasunorihirakawa 3:efc081576a33 78 }
yasunorihirakawa 3:efc081576a33 79 else
yasunorihirakawa 3:efc081576a33 80 {
yasunorihirakawa 3:efc081576a33 81 if( reflectorFM > 0.4 )
yasunorihirakawa 3:efc081576a33 82 {
yasunorihirakawa 3:efc081576a33 83 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 84 motorL.Set_target(fast_rpm);
yasunorihirakawa 3:efc081576a33 85 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 86 motorR.Set_target(fast_rpm);
yasunorihirakawa 3:efc081576a33 87 }
yasunorihirakawa 3:efc081576a33 88 else
yasunorihirakawa 3:efc081576a33 89 {
yasunorihirakawa 3:efc081576a33 90 if( reflectorFL.read() >= reflectorFR.read() )
yasunorihirakawa 3:efc081576a33 91 {
yasunorihirakawa 3:efc081576a33 92 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 93 motorR.Set_target(standard_rpm + KP * reflectorFL.read() + KD * (reflectorFL.read() - pre));
yasunorihirakawa 3:efc081576a33 94 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 95 motorL.Set_target(standard_rpm - KP * reflectorFL.read() + KD * (reflectorFL.read() - pre));
yasunorihirakawa 3:efc081576a33 96 pre = reflectorFL.read();
yasunorihirakawa 3:efc081576a33 97 flag = L;
yasunorihirakawa 3:efc081576a33 98 }
yasunorihirakawa 3:efc081576a33 99 else
yasunorihirakawa 3:efc081576a33 100 {
yasunorihirakawa 3:efc081576a33 101 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 102 motorR.Set_target(standard_rpm - KP * reflectorFR.read() + KD * (reflectorFR.read() - pre));
yasunorihirakawa 3:efc081576a33 103 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 104 motorL.Set_target(standard_rpm + KP * reflectorFR.read() + KD * (reflectorFR.read() - pre));
yasunorihirakawa 3:efc081576a33 105 pre = reflectorFR.read();
yasunorihirakawa 3:efc081576a33 106 flag = R;
yasunorihirakawa 3:efc081576a33 107 }
yasunorihirakawa 3:efc081576a33 108 }
yasunorihirakawa 3:efc081576a33 109 wait(INTVAL_REFLECTOR);
yasunorihirakawa 3:efc081576a33 110 }
yasunorihirakawa 3:efc081576a33 111
yasunorihirakawa 3:efc081576a33 112 printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
yasunorihirakawa 3:efc081576a33 113 }
yasunorihirakawa 3:efc081576a33 114 }
yasunorihirakawa 3:efc081576a33 115
yasunorihirakawa 3:efc081576a33 116 /*
yasunorihirakawa 3:efc081576a33 117 void line_trace()
yasunorihirakawa 3:efc081576a33 118 {
yasunorihirakawa 3:efc081576a33 119 //const float fast_rpm = 0;
yasunorihirakawa 3:efc081576a33 120 //const float standard_rpm = 0;
yasunorihirakawa 3:efc081576a33 121 //const float slow_rpm = 0;
yasunorihirakawa 3:efc081576a33 122 static float gap_pre = 0.0;
yasunorihirakawa 3:efc081576a33 123 const float KP = 700;//850;
yasunorihirakawa 3:efc081576a33 124 const float KD = 500;
yasunorihirakawa 3:efc081576a33 125 const int L = 0;
yasunorihirakawa 3:efc081576a33 126 const int R = 1;
yasunorihirakawa 3:efc081576a33 127
yasunorihirakawa 3:efc081576a33 128 int flag;
yasunorihirakawa 3:efc081576a33 129
yasunorihirakawa 3:efc081576a33 130 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 131 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 132
yasunorihirakawa 3:efc081576a33 133 while(1)
yasunorihirakawa 3:efc081576a33 134 {
yasunorihirakawa 3:efc081576a33 135 if( reflectorF<=0.4 && reflectorR<=0.4 && reflectorL<= 0.4 )
yasunorihirakawa 3:efc081576a33 136 {
yasunorihirakawa 3:efc081576a33 137 if( flag == L )
yasunorihirakawa 3:efc081576a33 138 {
yasunorihirakawa 3:efc081576a33 139 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 140 motorL.Set_target(0);
yasunorihirakawa 3:efc081576a33 141 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 142 motorR.Set_target(standard_rpm);
yasunorihirakawa 3:efc081576a33 143 //printf("turnL\n");
yasunorihirakawa 3:efc081576a33 144 }
yasunorihirakawa 3:efc081576a33 145 else
yasunorihirakawa 3:efc081576a33 146 {
yasunorihirakawa 3:efc081576a33 147 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 148 motorL.Set_target(standard_rpm);
yasunorihirakawa 3:efc081576a33 149 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 150 motorR.Set_target(0);
yasunorihirakawa 1:bb85c9fe1ba3 151 //printf("turnR\n");
yasunorihirakawa 1:bb85c9fe1ba3 152 }
yasunorihirakawa 1:bb85c9fe1ba3 153 }
yasunorihirakawa 1:bb85c9fe1ba3 154 else
yasunorihirakawa 1:bb85c9fe1ba3 155 {
yasunorihirakawa 1:bb85c9fe1ba3 156 if( reflectorF > 0.4 )
yasunorihirakawa 1:bb85c9fe1ba3 157 {
yasunorihirakawa 1:bb85c9fe1ba3 158 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 159 motorL.Set_target(fast_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 160 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 161 motorR.Set_target(fast_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 162 }
yasunorihirakawa 1:bb85c9fe1ba3 163 else
yasunorihirakawa 1:bb85c9fe1ba3 164 {
yasunorihirakawa 1:bb85c9fe1ba3 165 if( reflectorL.read() >= reflectorR.read() )
yasunorihirakawa 1:bb85c9fe1ba3 166 {
yasunorihirakawa 1:bb85c9fe1ba3 167 float voltage_gap = reflectorL.read() - reflectorR.read();
yasunorihirakawa 1:bb85c9fe1ba3 168 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 169 motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 1:bb85c9fe1ba3 170 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 171 motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 3:efc081576a33 172 gap_pre = voltage_gap;
yasunorihirakawa 1:bb85c9fe1ba3 173 flag = L;
yasunorihirakawa 1:bb85c9fe1ba3 174 }
yasunorihirakawa 1:bb85c9fe1ba3 175 else
yasunorihirakawa 1:bb85c9fe1ba3 176 {
yasunorihirakawa 1:bb85c9fe1ba3 177 float voltage_gap = reflectorR.read() - reflectorL.read();
yasunorihirakawa 1:bb85c9fe1ba3 178 motorR.Set_phase(1);
yasunorihirakawa 3:efc081576a33 179 motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 1:bb85c9fe1ba3 180 motorL.Set_phase(0);
yasunorihirakawa 3:efc081576a33 181 motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 3:efc081576a33 182 gap_pre = voltage_gap;
yasunorihirakawa 1:bb85c9fe1ba3 183 flag = R;
yasunorihirakawa 1:bb85c9fe1ba3 184 }
yasunorihirakawa 1:bb85c9fe1ba3 185 }
yasunorihirakawa 1:bb85c9fe1ba3 186 wait(INTVAL_REFLECTOR);
yasunorihirakawa 1:bb85c9fe1ba3 187 }
yasunorihirakawa 1:bb85c9fe1ba3 188
yasunorihirakawa 1:bb85c9fe1ba3 189 //printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
yasunorihirakawa 1:bb85c9fe1ba3 190 }
yasunorihirakawa 3:efc081576a33 191 }*/
Arima 0:532ef32974cf 192
yasunorihirakawa 1:bb85c9fe1ba3 193 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 194 {
yasunorihirakawa 1:bb85c9fe1ba3 195 const float pi = 3.14159265359;
yasunorihirakawa 1:bb85c9fe1ba3 196 const float gear_ratio = 1/38.2;
yasunorihirakawa 1:bb85c9fe1ba3 197 const float tire_size = 57.0/2;
yasunorihirakawa 1:bb85c9fe1ba3 198
yasunorihirakawa 1:bb85c9fe1ba3 199 static float v_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 200 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 201
yasunorihirakawa 1:bb85c9fe1ba3 202 float vl = motorL.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
yasunorihirakawa 1:bb85c9fe1ba3 203 float vr = motorR.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
yasunorihirakawa 1:bb85c9fe1ba3 204 float v = (vl + vr)/2;
yasunorihirakawa 1:bb85c9fe1ba3 205
yasunorihirakawa 1:bb85c9fe1ba3 206 dist += ( v + v_pre ) * INTVAL_MOTOR/2;
yasunorihirakawa 1:bb85c9fe1ba3 207 dist_count += ( v + v_pre ) * INTVAL_MOTOR/2;
yasunorihirakawa 1:bb85c9fe1ba3 208 v_pre = v;
yasunorihirakawa 1:bb85c9fe1ba3 209
yasunorihirakawa 3:efc081576a33 210 if (dist<=2000)
yasunorihirakawa 3:efc081576a33 211 {
yasunorihirakawa 3:efc081576a33 212 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 213 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 214 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 215 }
yasunorihirakawa 3:efc081576a33 216 else if(dist<=2400)
yasunorihirakawa 3:efc081576a33 217 {
yasunorihirakawa 3:efc081576a33 218 fast_rpm = 2000;
yasunorihirakawa 3:efc081576a33 219 standard_rpm = 1500;
yasunorihirakawa 3:efc081576a33 220 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 221 }
yasunorihirakawa 3:efc081576a33 222 else if(dist<=4400)
yasunorihirakawa 3:efc081576a33 223 {
yasunorihirakawa 3:efc081576a33 224 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 225 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 226 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 227 }
yasunorihirakawa 3:efc081576a33 228 else
yasunorihirakawa 3:efc081576a33 229 {
yasunorihirakawa 3:efc081576a33 230 fast_rpm = 1500;
yasunorihirakawa 3:efc081576a33 231 standard_rpm = 1200;
yasunorihirakawa 3:efc081576a33 232 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 233 }
yasunorihirakawa 2:09ea66e396c1 234
yasunorihirakawa 3:efc081576a33 235 //printf("%f\n", dist);
yasunorihirakawa 1:bb85c9fe1ba3 236
yasunorihirakawa 1:bb85c9fe1ba3 237 if( dist_count >= 200 )
yasunorihirakawa 1:bb85c9fe1ba3 238 {
yasunorihirakawa 1:bb85c9fe1ba3 239 LED = !LED;
yasunorihirakawa 1:bb85c9fe1ba3 240 dist_count -= 200;
Arima 0:532ef32974cf 241 }
yasunorihirakawa 1:bb85c9fe1ba3 242
yasunorihirakawa 1:bb85c9fe1ba3 243 if(dist>2147483000)
yasunorihirakawa 1:bb85c9fe1ba3 244 {
yasunorihirakawa 1:bb85c9fe1ba3 245 dist = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 246 }
yasunorihirakawa 1:bb85c9fe1ba3 247 }
yasunorihirakawa 1:bb85c9fe1ba3 248
yasunorihirakawa 1:bb85c9fe1ba3 249 void control_motor()
yasunorihirakawa 1:bb85c9fe1ba3 250 {
yasunorihirakawa 1:bb85c9fe1ba3 251 while(1)
yasunorihirakawa 1:bb85c9fe1ba3 252 {
yasunorihirakawa 1:bb85c9fe1ba3 253 motorL.drive();
yasunorihirakawa 1:bb85c9fe1ba3 254 motorR.drive();
yasunorihirakawa 1:bb85c9fe1ba3 255 odometry();
yasunorihirakawa 1:bb85c9fe1ba3 256 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 257 }
yasunorihirakawa 1:bb85c9fe1ba3 258 }
yasunorihirakawa 1:bb85c9fe1ba3 259
yasunorihirakawa 1:bb85c9fe1ba3 260 int main()
yasunorihirakawa 1:bb85c9fe1ba3 261 {
yasunorihirakawa 1:bb85c9fe1ba3 262 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 263 LED = 1;
yasunorihirakawa 1:bb85c9fe1ba3 264
yasunorihirakawa 1:bb85c9fe1ba3 265 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 266 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 267 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 268 pg2.fall(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 269
yasunorihirakawa 1:bb85c9fe1ba3 270 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 271 thread_motor.start(callback(control_motor));
Arima 0:532ef32974cf 272 }