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@9:87df9f0ec367, 2020-01-16 (annotated)

Committer:
yasunorihirakawa
Date:
Thu Jan 16 05:09:25 2020 +0000
Revision:
9:87df9f0ec367
Parent:
8:1c1c72c69af4 
aaaa

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 6:c024519cce11 6 Motor motorL(D5, D6);
yasunorihirakawa 6:c024519cce11 7 Motor motorR(D3, D4);
yasunorihirakawa 6:c024519cce11 8 InterruptIn pg1(D12);
yasunorihirakawa 6:c024519cce11 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);
153r173067 5:17314fc7b175 15
yasunorihirakawa 1:bb85c9fe1ba3 16 Thread thread_trace;
yasunorihirakawa 1:bb85c9fe1ba3 17 Thread thread_motor;
153r173067 5:17314fc7b175 18
yasunorihirakawa 9:87df9f0ec367 19 const float INTVAL_REFLECTOR(0.01);//0.01);
yasunorihirakawa 9:87df9f0ec367 20 const float INTVAL_MOTOR(0.01);
yasunorihirakawa 6:c024519cce11 21
yasunorihirakawa 1:bb85c9fe1ba3 22 float dist = 0.0;
yasunorihirakawa 4:5588f67b8c48 23 float x = 0.0;
yasunorihirakawa 4:5588f67b8c48 24 float y = 0.0;
153r173067 5:17314fc7b175 25 float fast_rpm = 0;
yasunorihirakawa 2:09ea66e396c1 26 float standard_rpm = 0;
153r173067 5:17314fc7b175 27 float slow_rpm = 0;
153r173067 5:17314fc7b175 28
yasunorihirakawa 9:87df9f0ec367 29 int curve = 0;
yasunorihirakawa 1:bb85c9fe1ba3 30
yasunorihirakawa 1:bb85c9fe1ba3 31 void count_pg1()
yasunorihirakawa 1:bb85c9fe1ba3 32 {
yasunorihirakawa 1:bb85c9fe1ba3 33 motorL.count();
yasunorihirakawa 1:bb85c9fe1ba3 34 }
yasunorihirakawa 1:bb85c9fe1ba3 35
yasunorihirakawa 1:bb85c9fe1ba3 36 void count_pg2()
yasunorihirakawa 1:bb85c9fe1ba3 37 {
yasunorihirakawa 1:bb85c9fe1ba3 38 motorR.count();
153r173067 5:17314fc7b175 39 }
yasunorihirakawa 1:bb85c9fe1ba3 40
yasunorihirakawa 1:bb85c9fe1ba3 41 void line_trace()
Arima 0:532ef32974cf 42 {
yasunorihirakawa 3:efc081576a33 43 //const float fast_rpm = 0;
yasunorihirakawa 3:efc081576a33 44 //const float standard_rpm = 0;
yasunorihirakawa 3:efc081576a33 45 //const float slow_rpm = 0;
yasunorihirakawa 4:5588f67b8c48 46 static float gap_pre = 0.0;
yasunorihirakawa 9:87df9f0ec367 47 const float KP = 700;
yasunorihirakawa 9:87df9f0ec367 48 const float KD = 100;
yasunorihirakawa 1:bb85c9fe1ba3 49 const int L = 0;
yasunorihirakawa 1:bb85c9fe1ba3 50 const int R = 1;
153r173067 5:17314fc7b175 51
yasunorihirakawa 1:bb85c9fe1ba3 52 int flag;
153r173067 5:17314fc7b175 53
yasunorihirakawa 4:5588f67b8c48 54 motorL.Set_phase(L);
yasunorihirakawa 4:5588f67b8c48 55 motorR.Set_phase(R);
153r173067 5:17314fc7b175 56
yasunorihirakawa 6:c024519cce11 57 while (1)
153r173067 5:17314fc7b175 58 {
yasunorihirakawa 9:87df9f0ec367 59 if(true)//curve==0)
yasunorihirakawa 6:c024519cce11 60 {
yasunorihirakawa 9:87df9f0ec367 61 if(reflectorFL > 0.1)
yasunorihirakawa 9:87df9f0ec367 62 {
yasunorihirakawa 9:87df9f0ec367 63 flag = L;
yasunorihirakawa 9:87df9f0ec367 64 }
yasunorihirakawa 9:87df9f0ec367 65 if(reflectorFR > 0.1)
yasunorihirakawa 6:c024519cce11 66 {
yasunorihirakawa 9:87df9f0ec367 67 flag = R;
yasunorihirakawa 9:87df9f0ec367 68 }
yasunorihirakawa 9:87df9f0ec367 69
yasunorihirakawa 9:87df9f0ec367 70 if( reflectorFM<=0.4 )
yasunorihirakawa 9:87df9f0ec367 71 {
yasunorihirakawa 9:87df9f0ec367 72 if( flag == L )
yasunorihirakawa 9:87df9f0ec367 73 {
yasunorihirakawa 9:87df9f0ec367 74 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 75 motorL.Set_target(0);
yasunorihirakawa 9:87df9f0ec367 76 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 77 motorR.Set_target(standard_rpm);
yasunorihirakawa 9:87df9f0ec367 78 }
yasunorihirakawa 9:87df9f0ec367 79 else
yasunorihirakawa 9:87df9f0ec367 80 {
yasunorihirakawa 9:87df9f0ec367 81 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 82 motorL.Set_target(standard_rpm);
yasunorihirakawa 9:87df9f0ec367 83 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 84 motorR.Set_target(0);
yasunorihirakawa 9:87df9f0ec367 85 }
yasunorihirakawa 6:c024519cce11 86 }
yasunorihirakawa 6:c024519cce11 87 else
yasunorihirakawa 6:c024519cce11 88 {
yasunorihirakawa 9:87df9f0ec367 89 if( reflectorFL>0.1 && reflectorFR>0.1 )
yasunorihirakawa 9:87df9f0ec367 90 {
yasunorihirakawa 9:87df9f0ec367 91 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 92 motorL.Set_target(fast_rpm);
yasunorihirakawa 9:87df9f0ec367 93 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 94 motorR.Set_target(fast_rpm);
yasunorihirakawa 9:87df9f0ec367 95 }
yasunorihirakawa 9:87df9f0ec367 96 else
yasunorihirakawa 9:87df9f0ec367 97 {
yasunorihirakawa 9:87df9f0ec367 98 if( reflectorBL.read() >= reflectorBR.read() )
yasunorihirakawa 9:87df9f0ec367 99 {
yasunorihirakawa 9:87df9f0ec367 100 float voltage_gap = reflectorBL.read() - reflectorBR.read();
yasunorihirakawa 9:87df9f0ec367 101 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 102 motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 103 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 104 motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 105 gap_pre = voltage_gap;
yasunorihirakawa 9:87df9f0ec367 106 }
yasunorihirakawa 9:87df9f0ec367 107 else
yasunorihirakawa 9:87df9f0ec367 108 {
yasunorihirakawa 9:87df9f0ec367 109 float voltage_gap = reflectorBR.read() - reflectorBL.read();
yasunorihirakawa 9:87df9f0ec367 110 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 111 motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 112 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 113 motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 114 gap_pre = voltage_gap;
yasunorihirakawa 9:87df9f0ec367 115 }
yasunorihirakawa 9:87df9f0ec367 116 }
yasunorihirakawa 6:c024519cce11 117 }
yasunorihirakawa 6:c024519cce11 118 }
yasunorihirakawa 6:c024519cce11 119 else
yasunorihirakawa 6:c024519cce11 120 {
yasunorihirakawa 9:87df9f0ec367 121 if( reflectorFM<=0.4 )
yasunorihirakawa 9:87df9f0ec367 122 {
yasunorihirakawa 9:87df9f0ec367 123 if( flag == L )
yasunorihirakawa 6:c024519cce11 124 {
yasunorihirakawa 9:87df9f0ec367 125 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 126 motorL.Set_target(0);
yasunorihirakawa 9:87df9f0ec367 127 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 128 motorR.Set_target(standard_rpm);
yasunorihirakawa 6:c024519cce11 129 }
yasunorihirakawa 6:c024519cce11 130 else
yasunorihirakawa 6:c024519cce11 131 {
yasunorihirakawa 9:87df9f0ec367 132 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 133 motorL.Set_target(standard_rpm);
yasunorihirakawa 9:87df9f0ec367 134 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 135 motorR.Set_target(0);
yasunorihirakawa 9:87df9f0ec367 136 }
yasunorihirakawa 6:c024519cce11 137 }
yasunorihirakawa 9:87df9f0ec367 138 else
yasunorihirakawa 1:bb85c9fe1ba3 139 {
yasunorihirakawa 9:87df9f0ec367 140 if( reflectorFL>0.1 && reflectorFR>0.1 )
yasunorihirakawa 9:87df9f0ec367 141 {
yasunorihirakawa 9:87df9f0ec367 142 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 143 motorL.Set_target(fast_rpm);
yasunorihirakawa 9:87df9f0ec367 144 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 145 motorR.Set_target(fast_rpm);
yasunorihirakawa 9:87df9f0ec367 146 }
yasunorihirakawa 9:87df9f0ec367 147 else
yasunorihirakawa 9:87df9f0ec367 148 {
yasunorihirakawa 9:87df9f0ec367 149 if( reflectorBL.read() >= reflectorBR.read() )
yasunorihirakawa 9:87df9f0ec367 150 {
yasunorihirakawa 9:87df9f0ec367 151 float voltage_gap = reflectorBL.read() - reflectorBR.read();
yasunorihirakawa 9:87df9f0ec367 152 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 153 motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 154 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 155 motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 156 gap_pre = voltage_gap;
yasunorihirakawa 9:87df9f0ec367 157 }
yasunorihirakawa 9:87df9f0ec367 158 else
yasunorihirakawa 9:87df9f0ec367 159 {
yasunorihirakawa 9:87df9f0ec367 160 float voltage_gap = reflectorBR.read() - reflectorBL.read();
yasunorihirakawa 9:87df9f0ec367 161 motorR.Set_phase(R);
yasunorihirakawa 9:87df9f0ec367 162 motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 163 motorL.Set_phase(L);
yasunorihirakawa 9:87df9f0ec367 164 motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
yasunorihirakawa 9:87df9f0ec367 165 gap_pre = voltage_gap;
yasunorihirakawa 9:87df9f0ec367 166 }
yasunorihirakawa 9:87df9f0ec367 167 }
yasunorihirakawa 4:5588f67b8c48 168 }
yasunorihirakawa 4:5588f67b8c48 169 }
yasunorihirakawa 6:c024519cce11 170
yasunorihirakawa 9:87df9f0ec367 171 //printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
yasunorihirakawa 4:5588f67b8c48 172 wait(INTVAL_REFLECTOR);
yasunorihirakawa 6:c024519cce11 173 }
yasunorihirakawa 3:efc081576a33 174 }
yasunorihirakawa 3:efc081576a33 175
yasunorihirakawa 1:bb85c9fe1ba3 176 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 177 {
yasunorihirakawa 9:87df9f0ec367 178 const float pi = 3.14159265359;
yasunorihirakawa 9:87df9f0ec367 179 const float gear_ratio = 1 / 38.2;
yasunorihirakawa 9:87df9f0ec367 180 const float tire_size = 57.0 / 2;
yasunorihirakawa 9:87df9f0ec367 181
153r173067 5:17314fc7b175 182 static float v_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 183 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 184
153r173067 5:17314fc7b175 185 float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 186 float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size;
153r173067 5:17314fc7b175 187 float v = (vl + vr) / 2;
153r173067 5:17314fc7b175 188
153r173067 5:17314fc7b175 189 dist += (v + v_pre) * INTVAL_MOTOR / 2;
153r173067 5:17314fc7b175 190 dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
yasunorihirakawa 1:bb85c9fe1ba3 191 v_pre = v;
153r173067 5:17314fc7b175 192
yasunorihirakawa 9:87df9f0ec367 193 if (dist<=6850)
yasunorihirakawa 3:efc081576a33 194 {
yasunorihirakawa 9:87df9f0ec367 195 fast_rpm = 5000;
yasunorihirakawa 9:87df9f0ec367 196 standard_rpm = 4500;
yasunorihirakawa 9:87df9f0ec367 197 curve = 0;
yasunorihirakawa 3:efc081576a33 198 }
yasunorihirakawa 9:87df9f0ec367 199 else if(dist<=7350)
yasunorihirakawa 3:efc081576a33 200 {
yasunorihirakawa 9:87df9f0ec367 201 fast_rpm = 4000;
yasunorihirakawa 9:87df9f0ec367 202 standard_rpm = 4000;
yasunorihirakawa 9:87df9f0ec367 203 curve = 0;
yasunorihirakawa 3:efc081576a33 204 }
yasunorihirakawa 3:efc081576a33 205 else
yasunorihirakawa 3:efc081576a33 206 {
yasunorihirakawa 9:87df9f0ec367 207 fast_rpm = 5000;
yasunorihirakawa 9:87df9f0ec367 208 standard_rpm = 4500;
yasunorihirakawa 9:87df9f0ec367 209 curve = 0;
yasunorihirakawa 9:87df9f0ec367 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 1:bb85c9fe1ba3 227 odometry();
yasunorihirakawa 1:bb85c9fe1ba3 228 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 229 }
153r173067 5:17314fc7b175 230 }
153r173067 5:17314fc7b175 231
yasunorihirakawa 1:bb85c9fe1ba3 232 int main()
yasunorihirakawa 9:87df9f0ec367 233 {
yasunorihirakawa 1:bb85c9fe1ba3 234 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 235 LED = 1;
yasunorihirakawa 9:87df9f0ec367 236
yasunorihirakawa 1:bb85c9fe1ba3 237 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 238 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 239 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 240 pg2.fall(&count_pg2);
yasunorihirakawa 9:87df9f0ec367 241
yasunorihirakawa 6:c024519cce11 242 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 243 thread_motor.start(callback(control_motor));
Arima 0:532ef32974cf 244 }