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@5:17314fc7b175, 2020-01-13 (annotated)

Committer:
153r173067
Date:
Mon Jan 13 07:07:47 2020 +0000
Revision:
5:17314fc7b175
Parent:
4:5588f67b8c48
Child:
6:c024519cce11
lgjhjkv

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