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

Committer:
yasunorihirakawa
Date:
Thu Jan 16 05:13:02 2020 +0000
Revision:
10:cea77ea5af93
Parent:
8:1c1c72c69af4
Child:
11:5ae0c22d5473
aa

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 10:cea77ea5af93 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 10:cea77ea5af93 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 10:cea77ea5af93 140 /*
yasunorihirakawa 4:5588f67b8c48 141 float gap_min = 0.0;
yasunorihirakawa 4:5588f67b8c48 142 int i_min = 0;
yasunorihirakawa 6:c024519cce11 143 for (int i = 0; i < 478; i++)
yasunorihirakawa 4:5588f67b8c48 144 {
yasunorihirakawa 6:c024519cce11 145 float dist_gap = dist - way[i][2];
153r173067 5:17314fc7b175 146 if (dist_gap < gap_min)
yasunorihirakawa 1:bb85c9fe1ba3 147 {
yasunorihirakawa 4:5588f67b8c48 148 gap_min = dist_gap;
yasunorihirakawa 4:5588f67b8c48 149 i_min = i;
yasunorihirakawa 4:5588f67b8c48 150 }
yasunorihirakawa 4:5588f67b8c48 151 }
yasunorihirakawa 6:c024519cce11 152
yasunorihirakawa 6:c024519cce11 153 float l = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) );
yasunorihirakawa 6:c024519cce11 154 float dv = K*l;
yasunorihirakawa 6:c024519cce11 155 float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size );
yasunorihirakawa 6:c024519cce11 156
yasunorihirakawa 6:c024519cce11 157 motorL.Set_target( standard_rpm + drpm );
yasunorihirakawa 6:c024519cce11 158 motorR.Set_target( standard_rpm - drpm );
yasunorihirakawa 10:cea77ea5af93 159 */
yasunorihirakawa 6:c024519cce11 160
153r173067 5:17314fc7b175 161 printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
yasunorihirakawa 4:5588f67b8c48 162 wait(INTVAL_REFLECTOR);
yasunorihirakawa 6:c024519cce11 163 }
yasunorihirakawa 3:efc081576a33 164 }
yasunorihirakawa 3:efc081576a33 165
yasunorihirakawa 1:bb85c9fe1ba3 166 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 167 {
153r173067 5:17314fc7b175 168 static float v_pre = 0.0;
153r173067 5:17314fc7b175 169 static float x_dot_pre = 0.0;
153r173067 5:17314fc7b175 170 static float y_dot_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 171 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 172
153r173067 5:17314fc7b175 173 float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 174 float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 175 float v = (vl + vr) / 2;
153r173067 5:17314fc7b175 176 float x_dot = v * cos(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 177 float y_dot = v * sin(bmx.get_azimuth_machineframe());
153r173067 5:17314fc7b175 178
153r173067 5:17314fc7b175 179 dist += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 180 dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 181 x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 8:1c1c72c69af4 182 y += (y_dot + y_dot_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 1:bb85c9fe1ba3 183 v_pre = v;
yasunorihirakawa 4:5588f67b8c48 184 x_dot_pre = x;
yasunorihirakawa 4:5588f67b8c48 185 y_dot_pre = y;
153r173067 5:17314fc7b175 186
yasunorihirakawa 3:efc081576a33 187 if (dist<=2000)
yasunorihirakawa 3:efc081576a33 188 {
yasunorihirakawa 3:efc081576a33 189 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 190 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 191 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 192 }
yasunorihirakawa 3:efc081576a33 193 else if(dist<=2400)
yasunorihirakawa 3:efc081576a33 194 {
yasunorihirakawa 3:efc081576a33 195 fast_rpm = 2000;
yasunorihirakawa 3:efc081576a33 196 standard_rpm = 1500;
yasunorihirakawa 3:efc081576a33 197 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 198 }
yasunorihirakawa 3:efc081576a33 199 else if(dist<=4400)
yasunorihirakawa 3:efc081576a33 200 {
yasunorihirakawa 3:efc081576a33 201 fast_rpm = 2500;
yasunorihirakawa 3:efc081576a33 202 standard_rpm = 2000;
yasunorihirakawa 3:efc081576a33 203 slow_rpm = 1200;
yasunorihirakawa 3:efc081576a33 204 }
yasunorihirakawa 3:efc081576a33 205 else
yasunorihirakawa 3:efc081576a33 206 {
yasunorihirakawa 3:efc081576a33 207 fast_rpm = 1500;
yasunorihirakawa 3:efc081576a33 208 standard_rpm = 1200;
yasunorihirakawa 3:efc081576a33 209 slow_rpm = 1200;
yasunorihirakawa 10:cea77ea5af93 210 }
153r173067 5:17314fc7b175 211
yasunorihirakawa 3:efc081576a33 212 //printf("%f\n", dist);
153r173067 5:17314fc7b175 213
153r173067 5:17314fc7b175 214 if (dist_count >= 200)
yasunorihirakawa 1:bb85c9fe1ba3 215 {
yasunorihirakawa 1:bb85c9fe1ba3 216 LED = !LED;
yasunorihirakawa 1:bb85c9fe1ba3 217 dist_count -= 200;
Arima 0:532ef32974cf 218 }
yasunorihirakawa 1:bb85c9fe1ba3 219 }
yasunorihirakawa 1:bb85c9fe1ba3 220
yasunorihirakawa 1:bb85c9fe1ba3 221 void control_motor()
yasunorihirakawa 1:bb85c9fe1ba3 222 {
153r173067 5:17314fc7b175 223 while (1)
yasunorihirakawa 1:bb85c9fe1ba3 224 {
yasunorihirakawa 1:bb85c9fe1ba3 225 motorL.drive();
yasunorihirakawa 1:bb85c9fe1ba3 226 motorR.drive();
yasunorihirakawa 10:cea77ea5af93 227 //odometry();
yasunorihirakawa 1:bb85c9fe1ba3 228 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 229 }
153r173067 5:17314fc7b175 230 }
153r173067 5:17314fc7b175 231
153r173067 5:17314fc7b175 232 void update()
153r173067 5:17314fc7b175 233 {
153r173067 5:17314fc7b175 234 while (true)
153r173067 5:17314fc7b175 235 {
153r173067 5:17314fc7b175 236 if (!lock)
153r173067 5:17314fc7b175 237 {
153r173067 5:17314fc7b175 238 bmx.Update_posture();
153r173067 5:17314fc7b175 239 }
153r173067 5:17314fc7b175 240 }
153r173067 5:17314fc7b175 241 }
yasunorihirakawa 1:bb85c9fe1ba3 242
yasunorihirakawa 1:bb85c9fe1ba3 243 int main()
153r173067 5:17314fc7b175 244 {
yasunorihirakawa 1:bb85c9fe1ba3 245 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 246 LED = 1;
yasunorihirakawa 1:bb85c9fe1ba3 247
yasunorihirakawa 1:bb85c9fe1ba3 248 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 249 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 250 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 251 pg2.fall(&count_pg2);
153r173067 5:17314fc7b175 252
153r173067 5:17314fc7b175 253 while (true)
153r173067 5:17314fc7b175 254 {
153r173067 5:17314fc7b175 255 if (button == 0)
153r173067 5:17314fc7b175 256 {
153r173067 5:17314fc7b175 257 wait(1);
153r173067 5:17314fc7b175 258 break;
153r173067 5:17314fc7b175 259 }
153r173067 5:17314fc7b175 260 }
153r173067 5:17314fc7b175 261
153r173067 5:17314fc7b175 262 bmx.mag_calibration(button);
153r173067 5:17314fc7b175 263
153r173067 5:17314fc7b175 264 printf("準備完了\n");
153r173067 5:17314fc7b175 265 while (true)
153r173067 5:17314fc7b175 266 {
153r173067 5:17314fc7b175 267 if (button == 0)
153r173067 5:17314fc7b175 268 {
153r173067 5:17314fc7b175 269 wait(1);
153r173067 5:17314fc7b175 270 break;
153r173067 5:17314fc7b175 271 }
153r173067 5:17314fc7b175 272 }
153r173067 5:17314fc7b175 273
153r173067 5:17314fc7b175 274 bmx.set_initial_mag();
153r173067 5:17314fc7b175 275 bmx.set_initial_acc();
153r173067 5:17314fc7b175 276
yasunorihirakawa 6:c024519cce11 277 sensor_thread.start(callback(update));
yasunorihirakawa 6:c024519cce11 278 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 279 thread_motor.start(callback(control_motor));
153r173067 5:17314fc7b175 280
yasunorihirakawa 6:c024519cce11 281 while (true)
153r173067 5:17314fc7b175 282 {
153r173067 5:17314fc7b175 283 if (button == 0)
153r173067 5:17314fc7b175 284 {
153r173067 5:17314fc7b175 285 bmx.posture.w = 1;
153r173067 5:17314fc7b175 286 bmx.posture.x = 0;
153r173067 5:17314fc7b175 287 bmx.posture.y = 0;
153r173067 5:17314fc7b175 288 bmx.posture.z = 0;
153r173067 5:17314fc7b175 289 lock = true;
153r173067 5:17314fc7b175 290 wait(0.1);
153r173067 5:17314fc7b175 291 bmx.set_initial_mag();
153r173067 5:17314fc7b175 292 bmx.set_initial_acc();
153r173067 5:17314fc7b175 293 lock = false;
153r173067 5:17314fc7b175 294 }
yasunorihirakawa 6:c024519cce11 295 }
Arima 0:532ef32974cf 296 }