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@7:ce2920da874b, 2020-01-15 (annotated)

Committer:
yasunorihirakawa
Date:
Wed Jan 15 02:25:54 2020 +0000
Revision:
7:ce2920da874b
Parent:
6:c024519cce11
Child:
8:1c1c72c69af4
a

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 6:c024519cce11 25 const float INTVAL_REFLECTOR(0.1);//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 6:c024519cce11 30 const float tire_size = 0.57 / 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 4:5588f67b8c48 58 const float KD = 3000;
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;
153r173067 5:17314fc7b175 72
yasunorihirakawa 6:c024519cce11 73 while (1)
153r173067 5:17314fc7b175 74 {
yasunorihirakawa 6:c024519cce11 75 /*
yasunorihirakawa 6:c024519cce11 76 if (reflectorFL>0.15)
153r173067 5:17314fc7b175 77 {
yasunorihirakawa 6:c024519cce11 78 flag=L;
yasunorihirakawa 6:c024519cce11 79 }
yasunorihirakawa 6:c024519cce11 80 if (reflectorFR>0.15)
yasunorihirakawa 6:c024519cce11 81 {
yasunorihirakawa 6:c024519cce11 82 flag=R;
153r173067 5:17314fc7b175 83 }
153r173067 5:17314fc7b175 84
yasunorihirakawa 6:c024519cce11 85 if ( reflectorFM<=0.4 && reflectorFL<=0.15 && reflectorFR<=0.15 )
yasunorihirakawa 6:c024519cce11 86 {
yasunorihirakawa 6:c024519cce11 87 if (flag==L)
yasunorihirakawa 6:c024519cce11 88 {
yasunorihirakawa 6:c024519cce11 89 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 90 motorL.Set_target(0);
yasunorihirakawa 6:c024519cce11 91 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 92 motorR.Set_target(slow_rpm);
yasunorihirakawa 6:c024519cce11 93 printf("turnL\n");
yasunorihirakawa 6:c024519cce11 94 }
yasunorihirakawa 6:c024519cce11 95 else
yasunorihirakawa 6:c024519cce11 96 {
yasunorihirakawa 6:c024519cce11 97 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 98 motorL.Set_target(slow_rpm);
yasunorihirakawa 6:c024519cce11 99 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 100 motorR.Set_target(0);
yasunorihirakawa 6:c024519cce11 101 printf("turnR\n");
yasunorihirakawa 6:c024519cce11 102 }
yasunorihirakawa 6:c024519cce11 103 }
yasunorihirakawa 6:c024519cce11 104 else
yasunorihirakawa 6:c024519cce11 105 {
yasunorihirakawa 6:c024519cce11 106 if ( reflectorFM>0.4 )
yasunorihirakawa 6:c024519cce11 107 {
yasunorihirakawa 6:c024519cce11 108 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 109 motorL.Set_target(fast_rpm);
yasunorihirakawa 6:c024519cce11 110 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 111 motorR.Set_target(fast_rpm);
yasunorihirakawa 6:c024519cce11 112 }
yasunorihirakawa 6:c024519cce11 113 else
yasunorihirakawa 6:c024519cce11 114 {
yasunorihirakawa 6:c024519cce11 115 if ( reflectorBL>0.15 )
yasunorihirakawa 6:c024519cce11 116 {
yasunorihirakawa 6:c024519cce11 117 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 118 motorL.Set_target(standard_rpm - standard_rpm*0.18);
yasunorihirakawa 6:c024519cce11 119 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 120 motorR.Set_target(standard_rpm + standard_rpm*0.18);
yasunorihirakawa 6:c024519cce11 121 }
yasunorihirakawa 6:c024519cce11 122 else
yasunorihirakawa 6:c024519cce11 123 {
yasunorihirakawa 6:c024519cce11 124 motorL.Set_phase(L);
yasunorihirakawa 6:c024519cce11 125 motorL.Set_target(standard_rpm + standard_rpm*0.18);
yasunorihirakawa 6:c024519cce11 126 motorR.Set_phase(R);
yasunorihirakawa 6:c024519cce11 127 motorR.Set_target(standard_rpm - standard_rpm*0.18);
yasunorihirakawa 6:c024519cce11 128 }
yasunorihirakawa 6:c024519cce11 129 }
yasunorihirakawa 6:c024519cce11 130 }*/
153r173067 5:17314fc7b175 131
yasunorihirakawa 4:5588f67b8c48 132 float gap_min = 0.0;
yasunorihirakawa 4:5588f67b8c48 133 int i_min = 0;
yasunorihirakawa 6:c024519cce11 134 for (int i = 0; i < 478; i++)
yasunorihirakawa 4:5588f67b8c48 135 {
yasunorihirakawa 6:c024519cce11 136 float dist_gap = dist - way[i][2];
153r173067 5:17314fc7b175 137 if (dist_gap < gap_min)
yasunorihirakawa 1:bb85c9fe1ba3 138 {
yasunorihirakawa 4:5588f67b8c48 139 gap_min = dist_gap;
yasunorihirakawa 4:5588f67b8c48 140 i_min = i;
yasunorihirakawa 4:5588f67b8c48 141 }
yasunorihirakawa 4:5588f67b8c48 142 }
yasunorihirakawa 6:c024519cce11 143
yasunorihirakawa 6:c024519cce11 144 float l = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) );
yasunorihirakawa 6:c024519cce11 145 float dv = K*l;
yasunorihirakawa 6:c024519cce11 146 float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size );
yasunorihirakawa 6:c024519cce11 147
yasunorihirakawa 6:c024519cce11 148 motorL.Set_target( standard_rpm + drpm );
yasunorihirakawa 6:c024519cce11 149 motorR.Set_target( standard_rpm - drpm );
yasunorihirakawa 6:c024519cce11 150
153r173067 5:17314fc7b175 151 printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
yasunorihirakawa 4:5588f67b8c48 152 wait(INTVAL_REFLECTOR);
yasunorihirakawa 6:c024519cce11 153 }
yasunorihirakawa 3:efc081576a33 154 }
yasunorihirakawa 3:efc081576a33 155
yasunorihirakawa 1:bb85c9fe1ba3 156 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 157 {
153r173067 5:17314fc7b175 158 static float v_pre = 0.0;
153r173067 5:17314fc7b175 159 static float x_dot_pre = 0.0;
153r173067 5:17314fc7b175 160 static float y_dot_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 161 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 162
153r173067 5:17314fc7b175 163 float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 164 float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 165 float v = (vl + vr) / 2;
153r173067 5:17314fc7b175 166 float x_dot = v * cos(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 167 float y_dot = v * sin(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 168
153r173067 5:17314fc7b175 169 dist += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 170 dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 171 x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 172 y += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 1:bb85c9fe1ba3 173 v_pre = v;
yasunorihirakawa 4:5588f67b8c48 174 x_dot_pre = x;
yasunorihirakawa 4:5588f67b8c48 175 y_dot_pre = y;
153r173067 5:17314fc7b175 176
yasunorihirakawa 6:c024519cce11 177 fast_rpm = 1200;
yasunorihirakawa 4:5588f67b8c48 178 standard_rpm = 1200;
153r173067 5:17314fc7b175 179 slow_rpm = 1200;
153r173067 5:17314fc7b175 180
yasunorihirakawa 4:5588f67b8c48 181 /*
yasunorihirakawa 3:efc081576a33 182 if (dist<=2000)
yasunorihirakawa 3:efc081576a33 183 {
yasunorihirakawa 3:efc081576a33 184 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 185 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 186 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 187 }
yasunorihirakawa 3:efc081576a33 188 else if(dist<=2400)
yasunorihirakawa 3:efc081576a33 189 {
yasunorihirakawa 3:efc081576a33 190 fast_rpm = 2000;
yasunorihirakawa 3:efc081576a33 191 standard_rpm = 1500;
yasunorihirakawa 3:efc081576a33 192 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 193 }
yasunorihirakawa 3:efc081576a33 194 else if(dist<=4400)
yasunorihirakawa 3:efc081576a33 195 {
yasunorihirakawa 3:efc081576a33 196 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 197 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 198 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 199 }
yasunorihirakawa 3:efc081576a33 200 else
yasunorihirakawa 3:efc081576a33 201 {
yasunorihirakawa 3:efc081576a33 202 fast_rpm = 1500;
yasunorihirakawa 3:efc081576a33 203 standard_rpm = 1200;
yasunorihirakawa 3:efc081576a33 204 slow_rpm = 1200;
yasunorihirakawa 4:5588f67b8c48 205 }*/
153r173067 5:17314fc7b175 206
yasunorihirakawa 3:efc081576a33 207 //printf("%f\n", dist);
153r173067 5:17314fc7b175 208
153r173067 5:17314fc7b175 209 if (dist_count >= 200)
yasunorihirakawa 1:bb85c9fe1ba3 210 {
yasunorihirakawa 1:bb85c9fe1ba3 211 LED = !LED;
yasunorihirakawa 1:bb85c9fe1ba3 212 dist_count -= 200;
Arima 0:532ef32974cf 213 }
yasunorihirakawa 1:bb85c9fe1ba3 214 }
yasunorihirakawa 1:bb85c9fe1ba3 215
yasunorihirakawa 1:bb85c9fe1ba3 216 void control_motor()
yasunorihirakawa 1:bb85c9fe1ba3 217 {
153r173067 5:17314fc7b175 218 while (1)
yasunorihirakawa 1:bb85c9fe1ba3 219 {
yasunorihirakawa 1:bb85c9fe1ba3 220 motorL.drive();
yasunorihirakawa 1:bb85c9fe1ba3 221 motorR.drive();
yasunorihirakawa 1:bb85c9fe1ba3 222 odometry();
yasunorihirakawa 1:bb85c9fe1ba3 223 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 224 }
153r173067 5:17314fc7b175 225 }
153r173067 5:17314fc7b175 226
153r173067 5:17314fc7b175 227 void update()
153r173067 5:17314fc7b175 228 {
153r173067 5:17314fc7b175 229 while (true)
153r173067 5:17314fc7b175 230 {
153r173067 5:17314fc7b175 231 if (!lock)
153r173067 5:17314fc7b175 232 {
153r173067 5:17314fc7b175 233 bmx.Update_posture();
153r173067 5:17314fc7b175 234 }
153r173067 5:17314fc7b175 235 }
153r173067 5:17314fc7b175 236 }
yasunorihirakawa 1:bb85c9fe1ba3 237
yasunorihirakawa 1:bb85c9fe1ba3 238 int main()
153r173067 5:17314fc7b175 239 {
yasunorihirakawa 1:bb85c9fe1ba3 240 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 241 LED = 1;
yasunorihirakawa 1:bb85c9fe1ba3 242
yasunorihirakawa 1:bb85c9fe1ba3 243 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 244 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 245 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 246 pg2.fall(&count_pg2);
153r173067 5:17314fc7b175 247
153r173067 5:17314fc7b175 248 while (true)
153r173067 5:17314fc7b175 249 {
153r173067 5:17314fc7b175 250 if (button == 0)
153r173067 5:17314fc7b175 251 {
153r173067 5:17314fc7b175 252 wait(1);
153r173067 5:17314fc7b175 253 break;
153r173067 5:17314fc7b175 254 }
153r173067 5:17314fc7b175 255 }
153r173067 5:17314fc7b175 256
153r173067 5:17314fc7b175 257 bmx.mag_calibration(button);
153r173067 5:17314fc7b175 258
153r173067 5:17314fc7b175 259 printf("準備完了\n");
153r173067 5:17314fc7b175 260 while (true)
153r173067 5:17314fc7b175 261 {
153r173067 5:17314fc7b175 262 if (button == 0)
153r173067 5:17314fc7b175 263 {
153r173067 5:17314fc7b175 264 wait(1);
153r173067 5:17314fc7b175 265 break;
153r173067 5:17314fc7b175 266 }
153r173067 5:17314fc7b175 267 }
153r173067 5:17314fc7b175 268
153r173067 5:17314fc7b175 269 bmx.set_initial_mag();
153r173067 5:17314fc7b175 270 bmx.set_initial_acc();
153r173067 5:17314fc7b175 271
yasunorihirakawa 6:c024519cce11 272 sensor_thread.start(callback(update));
yasunorihirakawa 6:c024519cce11 273 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 274 thread_motor.start(callback(control_motor));
153r173067 5:17314fc7b175 275
yasunorihirakawa 6:c024519cce11 276 while (true)
153r173067 5:17314fc7b175 277 {
153r173067 5:17314fc7b175 278 if (button == 0)
153r173067 5:17314fc7b175 279 {
153r173067 5:17314fc7b175 280 bmx.posture.w = 1;
153r173067 5:17314fc7b175 281 bmx.posture.x = 0;
153r173067 5:17314fc7b175 282 bmx.posture.y = 0;
153r173067 5:17314fc7b175 283 bmx.posture.z = 0;
153r173067 5:17314fc7b175 284 lock = true;
153r173067 5:17314fc7b175 285 wait(0.1);
153r173067 5:17314fc7b175 286 bmx.set_initial_mag();
153r173067 5:17314fc7b175 287 bmx.set_initial_acc();
153r173067 5:17314fc7b175 288 lock = false;
153r173067 5:17314fc7b175 289 }
yasunorihirakawa 6:c024519cce11 290 }
Arima 0:532ef32974cf 291 }