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@12:a4d7e7615fc3, 2020-01-16 (annotated)

Committer:
yasunorihirakawa
Date:
Thu Jan 16 05:17:01 2020 +0000
Revision:
12:a4d7e7615fc3
Parent:
11:5ae0c22d5473 
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yasunorihirakawa 4:5588f67b8c48 1 //本命
Arima 0:532ef32974cf 2 #include "mbed.h"
yasunorihirakawa 1:bb85c9fe1ba3 3 #include "motor.h"
153r173067 5:17314fc7b175 4 #include "BMX055.h"
yasunorihirakawa 6:c024519cce11 5 #include "way.h"
yasunorihirakawa 1:bb85c9fe1ba3 6
yasunorihirakawa 6:c024519cce11 7 BMX055 bmx(D0, D1);
yasunorihirakawa 6:c024519cce11 8 DigitalIn button(D2);
yasunorihirakawa 1:bb85c9fe1ba3 9 DigitalOut motor_mode(D9);
yasunorihirakawa 3:efc081576a33 10 DigitalOut LED(D10);
yasunorihirakawa 6:c024519cce11 11 Motor motorL(D5, D6);
yasunorihirakawa 6:c024519cce11 12 Motor motorR(D3, D4);
yasunorihirakawa 6:c024519cce11 13 InterruptIn pg1(D12);
yasunorihirakawa 6:c024519cce11 14 InterruptIn pg2(D11);
yasunorihirakawa 3:efc081576a33 15 AnalogIn reflectorFL(A6);
yasunorihirakawa 3:efc081576a33 16 AnalogIn reflectorFM(A3);
yasunorihirakawa 3:efc081576a33 17 AnalogIn reflectorFR(A2);
yasunorihirakawa 3:efc081576a33 18 AnalogIn reflectorBL(A1);
yasunorihirakawa 3:efc081576a33 19 AnalogIn reflectorBR(A0);
153r173067 5:17314fc7b175 20
yasunorihirakawa 1:bb85c9fe1ba3 21 Thread thread_trace;
yasunorihirakawa 1:bb85c9fe1ba3 22 Thread thread_motor;
153r173067 5:17314fc7b175 23 Thread sensor_thread;
153r173067 5:17314fc7b175 24
yasunorihirakawa 10:cea77ea5af93 25 const float INTVAL_REFLECTOR(0.01);//0.01);
153r173067 5:17314fc7b175 26 const float INTVAL_MOTOR(0.25);
yasunorihirakawa 1:bb85c9fe1ba3 27
yasunorihirakawa 6:c024519cce11 28 const float pi = 3.14159265359;
yasunorihirakawa 6:c024519cce11 29 const float gear_ratio = 1 / 38.2;
yasunorihirakawa 10:cea77ea5af93 30 const float tire_size = 0.057 / 2;
yasunorihirakawa 6:c024519cce11 31
yasunorihirakawa 1:bb85c9fe1ba3 32 float dist = 0.0;
yasunorihirakawa 4:5588f67b8c48 33 float x = 0.0;
yasunorihirakawa 4:5588f67b8c48 34 float y = 0.0;
153r173067 5:17314fc7b175 35 float fast_rpm = 0;
yasunorihirakawa 2:09ea66e396c1 36 float standard_rpm = 0;
153r173067 5:17314fc7b175 37 float slow_rpm = 0;
153r173067 5:17314fc7b175 38
153r173067 5:17314fc7b175 39 bool lock = false;
yasunorihirakawa 1:bb85c9fe1ba3 40
yasunorihirakawa 1:bb85c9fe1ba3 41 void count_pg1()
yasunorihirakawa 1:bb85c9fe1ba3 42 {
yasunorihirakawa 1:bb85c9fe1ba3 43 motorL.count();
yasunorihirakawa 1:bb85c9fe1ba3 44 }
yasunorihirakawa 1:bb85c9fe1ba3 45
yasunorihirakawa 1:bb85c9fe1ba3 46 void count_pg2()
yasunorihirakawa 1:bb85c9fe1ba3 47 {
yasunorihirakawa 1:bb85c9fe1ba3 48 motorR.count();
153r173067 5:17314fc7b175 49 }
yasunorihirakawa 1:bb85c9fe1ba3 50
yasunorihirakawa 1:bb85c9fe1ba3 51 void line_trace()
Arima 0:532ef32974cf 52 {
yasunorihirakawa 3:efc081576a33 53 //const float fast_rpm = 0;
yasunorihirakawa 3:efc081576a33 54 //const float standard_rpm = 0;
yasunorihirakawa 3:efc081576a33 55 //const float slow_rpm = 0;
yasunorihirakawa 4:5588f67b8c48 56 static float gap_pre = 0.0;
yasunorihirakawa 6:c024519cce11 57 const float KP = 850;
yasunorihirakawa 10:cea77ea5af93 58 const float KD = 0;
yasunorihirakawa 7:ce2920da874b 59 const float K = 0.01;
yasunorihirakawa 1:bb85c9fe1ba3 60 const int L = 0;
yasunorihirakawa 1:bb85c9fe1ba3 61 const int R = 1;
153r173067 5:17314fc7b175 62
yasunorihirakawa 1:bb85c9fe1ba3 63 int flag;
153r173067 5:17314fc7b175 64
yasunorihirakawa 4:5588f67b8c48 65 motorL.Set_phase(L);
yasunorihirakawa 4:5588f67b8c48 66 motorR.Set_phase(R);
153r173067 5:17314fc7b175 67
153r173067 5:17314fc7b175 68 float target_azimuth = 0.0;
153r173067 5:17314fc7b175 69 float current_azimuth = 0.0;
153r173067 5:17314fc7b175 70 float curvature = 0.0;
153r173067 5:17314fc7b175 71 float tread = 0.097;
yasunorihirakawa 10:cea77ea5af93 72
yasunorihirakawa 10:cea77ea5af93 73 fast_rpm = 1200;
yasunorihirakawa 10:cea77ea5af93 74 standard_rpm = 1200;
yasunorihirakawa 10:cea77ea5af93 75 slow_rpm = 1200;
153r173067 5:17314fc7b175 76
yasunorihirakawa 6:c024519cce11 77 while (1)
153r173067 5:17314fc7b175 78 {
yasunorihirakawa 11:5ae0c22d5473 79 if(reflectorFL > 0.1)
153r173067 5:17314fc7b175 80 {
yasunorihirakawa 10:cea77ea5af93 81 flag = L;
153r173067 5:17314fc7b175 82 }
yasunorihirakawa 10:cea77ea5af93 83 if(reflectorFR > 0.1)
yasunorihirakawa 6:c024519cce11 84 {
yasunorihirakawa 10:cea77ea5af93 85 flag = R;
yasunorihirakawa 11:5ae0c22d5473 86 }
yasunorihirakawa 10:cea77ea5af93 87 if( reflectorFM<=0.4 && reflectorBL<=0.1 && reflectorBR<=0.1 )//&& reflectorFL<=0.1 && reflectorFR<=0.1 )
yasunorihirakawa 10:cea77ea5af93 88 {
yasunorihirakawa 10:cea77ea5af93 89 if( flag == L )
yasunorihirakawa 6:c024519cce11 90 {
yasunorihirakawa 10:cea77ea5af93 91 motorL.Set_phase(L);
yasunorihirakawa 10:cea77ea5af93 92 motorL.Set_target(0);
yasunorihirakawa 10:cea77ea5af93 93 motorR.Set_phase(R);
yasunorihirakawa 10:cea77ea5af93 94 motorR.Set_target(slow_rpm);
yasunorihirakawa 10:cea77ea5af93 95 //printf("turnL\n");
yasunorihirakawa 6:c024519cce11 96 }
yasunorihirakawa 6:c024519cce11 97 else
yasunorihirakawa 6:c024519cce11 98 {
yasunorihirakawa 10:cea77ea5af93 99 motorL.Set_phase(L);
yasunorihirakawa 10:cea77ea5af93 100 motorL.Set_target(slow_rpm);
yasunorihirakawa 10:cea77ea5af93 101 motorR.Set_phase(R);
yasunorihirakawa 10:cea77ea5af93 102 motorR.Set_target(0);
yasunorihirakawa 10:cea77ea5af93 103 //printf("turnR\n");
yasunorihirakawa 10:cea77ea5af93 104 }
yasunorihirakawa 6:c024519cce11 105 }
yasunorihirakawa 6:c024519cce11 106 else
yasunorihirakawa 6:c024519cce11 107 {
yasunorihirakawa 10:cea77ea5af93 108 if( reflectorFM > 0.4 )
yasunorihirakawa 6:c024519cce11 109 {
yasunorihirakawa 6:c024519cce11 110 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 111 motorL.Set_target(fast_rpm);
yasunorihirakawa 6:c024519cce11 112 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 113 motorR.Set_target(fast_rpm);
yasunorihirakawa 6:c024519cce11 114 }
yasunorihirakawa 6:c024519cce11 115 else
yasunorihirakawa 6:c024519cce11 116 {
yasunorihirakawa 10:cea77ea5af93 117 if( reflectorBL.read() >= reflectorBR.read() )
yasunorihirakawa 6:c024519cce11 118 {
yasunorihirakawa 10:cea77ea5af93 119 float voltage_gap = reflectorBL.read() - reflectorBR.read();
yasunorihirakawa 10:cea77ea5af93 120 motorR.Set_phase(R);
yasunorihirakawa 10:cea77ea5af93 121 motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 6:c024519cce11 122 motorL.Set_phase(L);
yasunorihirakawa 10:cea77ea5af93 123 motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 10:cea77ea5af93 124 gap_pre = voltage_gap;
yasunorihirakawa 10:cea77ea5af93 125 flag = L;
yasunorihirakawa 6:c024519cce11 126 }
yasunorihirakawa 6:c024519cce11 127 else
yasunorihirakawa 6:c024519cce11 128 {
yasunorihirakawa 10:cea77ea5af93 129 float voltage_gap = reflectorBR.read() - reflectorBL.read();
yasunorihirakawa 6:c024519cce11 130 motorR.Set_phase(R);
yasunorihirakawa 10:cea77ea5af93 131 motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 10:cea77ea5af93 132 motorL.Set_phase(L);
yasunorihirakawa 10:cea77ea5af93 133 motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 10:cea77ea5af93 134 gap_pre = voltage_gap;
yasunorihirakawa 10:cea77ea5af93 135 flag = R;
yasunorihirakawa 6:c024519cce11 136 }
yasunorihirakawa 6:c024519cce11 137 }
yasunorihirakawa 10:cea77ea5af93 138 }
153r173067 5:17314fc7b175 139
yasunorihirakawa 11:5ae0c22d5473 140 float gap_min = 10000.0;
yasunorihirakawa 4:5588f67b8c48 141 int i_min = 0;
yasunorihirakawa 6:c024519cce11 142 for (int i = 0; i < 478; i++)
yasunorihirakawa 4:5588f67b8c48 143 {
yasunorihirakawa 6:c024519cce11 144 float dist_gap = dist - way[i][2];
153r173067 5:17314fc7b175 145 if (dist_gap < gap_min)
yasunorihirakawa 1:bb85c9fe1ba3 146 {
yasunorihirakawa 4:5588f67b8c48 147 gap_min = dist_gap;
yasunorihirakawa 4:5588f67b8c48 148 i_min = i;
yasunorihirakawa 4:5588f67b8c48 149 }
yasunorihirakawa 4:5588f67b8c48 150 }
yasunorihirakawa 6:c024519cce11 151
yasunorihirakawa 6:c024519cce11 152 float l = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) );
yasunorihirakawa 6:c024519cce11 153 float dv = K*l;
yasunorihirakawa 6:c024519cce11 154 float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size );
yasunorihirakawa 6:c024519cce11 155
yasunorihirakawa 6:c024519cce11 156 motorL.Set_target( standard_rpm + drpm );
yasunorihirakawa 6:c024519cce11 157 motorR.Set_target( standard_rpm - drpm );
yasunorihirakawa 10:cea77ea5af93 158 */
yasunorihirakawa 6:c024519cce11 159
153r173067 5:17314fc7b175 160 printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
yasunorihirakawa 4:5588f67b8c48 161 wait(INTVAL_REFLECTOR);
yasunorihirakawa 6:c024519cce11 162 }
yasunorihirakawa 3:efc081576a33 163 }
yasunorihirakawa 3:efc081576a33 164
yasunorihirakawa 1:bb85c9fe1ba3 165 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 166 {
153r173067 5:17314fc7b175 167 static float v_pre = 0.0;
153r173067 5:17314fc7b175 168 static float x_dot_pre = 0.0;
153r173067 5:17314fc7b175 169 static float y_dot_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 170 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 171
153r173067 5:17314fc7b175 172 float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 173 float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 174 float v = (vl + vr) / 2;
153r173067 5:17314fc7b175 175 float x_dot = v * cos(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 176 float y_dot = v * sin(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 177
153r173067 5:17314fc7b175 178 dist += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 179 dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 180 x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 8:1c1c72c69af4 181 y += (y_dot + y_dot_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 1:bb85c9fe1ba3 182 v_pre = v;
yasunorihirakawa 4:5588f67b8c48 183 x_dot_pre = x;
yasunorihirakawa 4:5588f67b8c48 184 y_dot_pre = y;
153r173067 5:17314fc7b175 185
yasunorihirakawa 3:efc081576a33 186 if (dist<=2000)
yasunorihirakawa 3:efc081576a33 187 {
yasunorihirakawa 3:efc081576a33 188 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 189 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 190 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 191 }
yasunorihirakawa 3:efc081576a33 192 else if(dist<=2400)
yasunorihirakawa 3:efc081576a33 193 {
yasunorihirakawa 3:efc081576a33 194 fast_rpm = 2000;
yasunorihirakawa 3:efc081576a33 195 standard_rpm = 1500;
yasunorihirakawa 3:efc081576a33 196 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 197 }
yasunorihirakawa 3:efc081576a33 198 else if(dist<=4400)
yasunorihirakawa 3:efc081576a33 199 {
yasunorihirakawa 3:efc081576a33 200 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 201 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 202 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 203 }
yasunorihirakawa 3:efc081576a33 204 else
yasunorihirakawa 3:efc081576a33 205 {
yasunorihirakawa 3:efc081576a33 206 fast_rpm = 1500;
yasunorihirakawa 3:efc081576a33 207 standard_rpm = 1200;
yasunorihirakawa 3:efc081576a33 208 slow_rpm = 1200;
yasunorihirakawa 10:cea77ea5af93 209 }
153r173067 5:17314fc7b175 210
yasunorihirakawa 3:efc081576a33 211 //printf("%f\n", dist);
153r173067 5:17314fc7b175 212
153r173067 5:17314fc7b175 213 if (dist_count >= 200)
yasunorihirakawa 1:bb85c9fe1ba3 214 {
yasunorihirakawa 1:bb85c9fe1ba3 215 LED = !LED;
yasunorihirakawa 1:bb85c9fe1ba3 216 dist_count -= 200;
Arima 0:532ef32974cf 217 }
yasunorihirakawa 1:bb85c9fe1ba3 218 }
yasunorihirakawa 1:bb85c9fe1ba3 219
yasunorihirakawa 1:bb85c9fe1ba3 220 void control_motor()
yasunorihirakawa 1:bb85c9fe1ba3 221 {
153r173067 5:17314fc7b175 222 while (1)
yasunorihirakawa 1:bb85c9fe1ba3 223 {
yasunorihirakawa 1:bb85c9fe1ba3 224 motorL.drive();
yasunorihirakawa 1:bb85c9fe1ba3 225 motorR.drive();
yasunorihirakawa 12:a4d7e7615fc3 226 odometry();
yasunorihirakawa 1:bb85c9fe1ba3 227 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 228 }
153r173067 5:17314fc7b175 229 }
153r173067 5:17314fc7b175 230
153r173067 5:17314fc7b175 231 void update()
153r173067 5:17314fc7b175 232 {
153r173067 5:17314fc7b175 233 while (true)
153r173067 5:17314fc7b175 234 {
153r173067 5:17314fc7b175 235 if (!lock)
153r173067 5:17314fc7b175 236 {
153r173067 5:17314fc7b175 237 bmx.Update_posture();
153r173067 5:17314fc7b175 238 }
153r173067 5:17314fc7b175 239 }
153r173067 5:17314fc7b175 240 }
yasunorihirakawa 1:bb85c9fe1ba3 241
yasunorihirakawa 1:bb85c9fe1ba3 242 int main()
153r173067 5:17314fc7b175 243 {
yasunorihirakawa 1:bb85c9fe1ba3 244 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 245 LED = 1;
yasunorihirakawa 1:bb85c9fe1ba3 246
yasunorihirakawa 1:bb85c9fe1ba3 247 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 248 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 249 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 250 pg2.fall(&count_pg2);
153r173067 5:17314fc7b175 251
153r173067 5:17314fc7b175 252 while (true)
153r173067 5:17314fc7b175 253 {
153r173067 5:17314fc7b175 254 if (button == 0)
153r173067 5:17314fc7b175 255 {
153r173067 5:17314fc7b175 256 wait(1);
153r173067 5:17314fc7b175 257 break;
153r173067 5:17314fc7b175 258 }
153r173067 5:17314fc7b175 259 }
153r173067 5:17314fc7b175 260
153r173067 5:17314fc7b175 261 bmx.mag_calibration(button);
153r173067 5:17314fc7b175 262
153r173067 5:17314fc7b175 263 printf("準備完了\n");
153r173067 5:17314fc7b175 264 while (true)
153r173067 5:17314fc7b175 265 {
153r173067 5:17314fc7b175 266 if (button == 0)
153r173067 5:17314fc7b175 267 {
153r173067 5:17314fc7b175 268 wait(1);
153r173067 5:17314fc7b175 269 break;
153r173067 5:17314fc7b175 270 }
153r173067 5:17314fc7b175 271 }
153r173067 5:17314fc7b175 272
153r173067 5:17314fc7b175 273 bmx.set_initial_mag();
153r173067 5:17314fc7b175 274 bmx.set_initial_acc();
153r173067 5:17314fc7b175 275
yasunorihirakawa 6:c024519cce11 276 sensor_thread.start(callback(update));
yasunorihirakawa 6:c024519cce11 277 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 278 thread_motor.start(callback(control_motor));
153r173067 5:17314fc7b175 279
yasunorihirakawa 6:c024519cce11 280 while (true)
153r173067 5:17314fc7b175 281 {
153r173067 5:17314fc7b175 282 if (button == 0)
153r173067 5:17314fc7b175 283 {
153r173067 5:17314fc7b175 284 bmx.posture.w = 1;
153r173067 5:17314fc7b175 285 bmx.posture.x = 0;
153r173067 5:17314fc7b175 286 bmx.posture.y = 0;
153r173067 5:17314fc7b175 287 bmx.posture.z = 0;
153r173067 5:17314fc7b175 288 lock = true;
153r173067 5:17314fc7b175 289 wait(0.1);
153r173067 5:17314fc7b175 290 bmx.set_initial_mag();
153r173067 5:17314fc7b175 291 bmx.set_initial_acc();
153r173067 5:17314fc7b175 292 lock = false;
153r173067 5:17314fc7b175 293 }
yasunorihirakawa 6:c024519cce11 294 }
Arima 0:532ef32974cf 295 }