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@2:09ea66e396c1, 2019-12-04 (annotated)

Committer:
yasunorihirakawa
Date:
Wed Dec 04 02:32:00 2019 +0000
Revision:
2:09ea66e396c1
Parent:
1:bb85c9fe1ba3
Child:
3:efc081576a33
;

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 1:bb85c9fe1ba3 5 DigitalOut LED(A6);
yasunorihirakawa 1:bb85c9fe1ba3 6 Motor motorL(D5, D6);
yasunorihirakawa 1:bb85c9fe1ba3 7 Motor motorR(D3, D4);
yasunorihirakawa 1:bb85c9fe1ba3 8 InterruptIn pg1(D12);
yasunorihirakawa 1:bb85c9fe1ba3 9 InterruptIn pg2(D11);
yasunorihirakawa 1:bb85c9fe1ba3 10 AnalogIn reflectorF(A2);
yasunorihirakawa 1:bb85c9fe1ba3 11 AnalogIn reflectorL(A1);
yasunorihirakawa 1:bb85c9fe1ba3 12 AnalogIn reflectorR(A0);
yasunorihirakawa 1:bb85c9fe1ba3 13
yasunorihirakawa 1:bb85c9fe1ba3 14 Thread thread_trace;
yasunorihirakawa 1:bb85c9fe1ba3 15 Thread thread_motor;
Arima 0:532ef32974cf 16
yasunorihirakawa 1:bb85c9fe1ba3 17 const int THREAD_FLAG_TRACE(1);
yasunorihirakawa 1:bb85c9fe1ba3 18 const int THREAD_FLAG_MOTOR(2);
yasunorihirakawa 1:bb85c9fe1ba3 19 const float INTVAL_REFLECTOR (0.01);
yasunorihirakawa 1:bb85c9fe1ba3 20 const float INTVAL_MOTOR (0.25);
yasunorihirakawa 1:bb85c9fe1ba3 21
yasunorihirakawa 1:bb85c9fe1ba3 22 float dist = 0.0;
yasunorihirakawa 2:09ea66e396c1 23 float standard_rpm = 0;
yasunorihirakawa 1:bb85c9fe1ba3 24
yasunorihirakawa 1:bb85c9fe1ba3 25 void count_pg1()
yasunorihirakawa 1:bb85c9fe1ba3 26 {
yasunorihirakawa 1:bb85c9fe1ba3 27 motorL.count();
yasunorihirakawa 1:bb85c9fe1ba3 28 }
yasunorihirakawa 1:bb85c9fe1ba3 29
yasunorihirakawa 1:bb85c9fe1ba3 30 void count_pg2()
yasunorihirakawa 1:bb85c9fe1ba3 31 {
yasunorihirakawa 1:bb85c9fe1ba3 32 motorR.count();
yasunorihirakawa 1:bb85c9fe1ba3 33 }
yasunorihirakawa 1:bb85c9fe1ba3 34
yasunorihirakawa 1:bb85c9fe1ba3 35 void line_trace()
Arima 0:532ef32974cf 36 {
yasunorihirakawa 2:09ea66e396c1 37 //const float fast_rpm = 2000;
yasunorihirakawa 2:09ea66e396c1 38 //const float standard_rpm = 1200;
yasunorihirakawa 2:09ea66e396c1 39 //const float slow_rpm = 1000;
yasunorihirakawa 2:09ea66e396c1 40 const float K = 750;//850;
yasunorihirakawa 1:bb85c9fe1ba3 41 const int L = 0;
yasunorihirakawa 1:bb85c9fe1ba3 42 const int R = 1;
yasunorihirakawa 1:bb85c9fe1ba3 43
yasunorihirakawa 1:bb85c9fe1ba3 44 int flag;
yasunorihirakawa 1:bb85c9fe1ba3 45
yasunorihirakawa 1:bb85c9fe1ba3 46 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 47 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 48
yasunorihirakawa 1:bb85c9fe1ba3 49 while(1)
yasunorihirakawa 1:bb85c9fe1ba3 50 {
yasunorihirakawa 2:09ea66e396c1 51 if( /*reflectorF<=0.4 &&*/ reflectorR<=0.4 && reflectorL<= 0.4 )
yasunorihirakawa 1:bb85c9fe1ba3 52 {
yasunorihirakawa 1:bb85c9fe1ba3 53 if( flag == L )
yasunorihirakawa 1:bb85c9fe1ba3 54 {
yasunorihirakawa 2:09ea66e396c1 55 motorL.Set_phase(0);//1);
yasunorihirakawa 2:09ea66e396c1 56 motorL.Set_target(0);//slow_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 57 motorR.Set_phase(1);
yasunorihirakawa 2:09ea66e396c1 58 motorR.Set_target(standard_rpm);//slow_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 59 //printf("turnL\n");
yasunorihirakawa 1:bb85c9fe1ba3 60 }
yasunorihirakawa 1:bb85c9fe1ba3 61 else
yasunorihirakawa 1:bb85c9fe1ba3 62 {
yasunorihirakawa 1:bb85c9fe1ba3 63 motorL.Set_phase(0);
yasunorihirakawa 2:09ea66e396c1 64 motorL.Set_target(standard_rpm);//slow_rpm);
yasunorihirakawa 2:09ea66e396c1 65 motorR.Set_phase(1);//0);
yasunorihirakawa 2:09ea66e396c1 66 motorR.Set_target(0);//slow_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 67 //printf("turnR\n");
yasunorihirakawa 1:bb85c9fe1ba3 68 }
yasunorihirakawa 1:bb85c9fe1ba3 69 }
yasunorihirakawa 1:bb85c9fe1ba3 70 else
yasunorihirakawa 1:bb85c9fe1ba3 71 {
yasunorihirakawa 1:bb85c9fe1ba3 72 if( reflectorF > 0.4 )
yasunorihirakawa 1:bb85c9fe1ba3 73 {
yasunorihirakawa 1:bb85c9fe1ba3 74 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 75 motorL.Set_target(fast_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 76 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 77 motorR.Set_target(fast_rpm);
yasunorihirakawa 1:bb85c9fe1ba3 78 }
yasunorihirakawa 1:bb85c9fe1ba3 79 else
yasunorihirakawa 1:bb85c9fe1ba3 80 {
yasunorihirakawa 1:bb85c9fe1ba3 81 if( reflectorL.read() >= reflectorR.read() )
yasunorihirakawa 1:bb85c9fe1ba3 82 {
yasunorihirakawa 1:bb85c9fe1ba3 83 float voltage_gap = reflectorL.read() - reflectorR.read();
yasunorihirakawa 1:bb85c9fe1ba3 84 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 85 motorR.Set_target(standard_rpm + K * voltage_gap);
yasunorihirakawa 1:bb85c9fe1ba3 86 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 87 motorL.Set_target(standard_rpm - K * voltage_gap);
yasunorihirakawa 1:bb85c9fe1ba3 88 flag = L;
yasunorihirakawa 1:bb85c9fe1ba3 89 }
yasunorihirakawa 1:bb85c9fe1ba3 90 else
yasunorihirakawa 1:bb85c9fe1ba3 91 {
yasunorihirakawa 1:bb85c9fe1ba3 92 float voltage_gap = reflectorR.read() - reflectorL.read();
yasunorihirakawa 1:bb85c9fe1ba3 93 motorR.Set_phase(1);
yasunorihirakawa 1:bb85c9fe1ba3 94 motorR.Set_target(standard_rpm - K * voltage_gap);
yasunorihirakawa 1:bb85c9fe1ba3 95 motorL.Set_phase(0);
yasunorihirakawa 1:bb85c9fe1ba3 96 motorL.Set_target(standard_rpm + K * voltage_gap);
yasunorihirakawa 1:bb85c9fe1ba3 97 flag = R;
yasunorihirakawa 1:bb85c9fe1ba3 98 }
yasunorihirakawa 1:bb85c9fe1ba3 99 }
yasunorihirakawa 1:bb85c9fe1ba3 100 wait(INTVAL_REFLECTOR);
yasunorihirakawa 1:bb85c9fe1ba3 101 }
yasunorihirakawa 1:bb85c9fe1ba3 102
yasunorihirakawa 1:bb85c9fe1ba3 103 //printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
yasunorihirakawa 1:bb85c9fe1ba3 104 }
yasunorihirakawa 1:bb85c9fe1ba3 105 }
Arima 0:532ef32974cf 106
yasunorihirakawa 1:bb85c9fe1ba3 107 void odometry()
yasunorihirakawa 1:bb85c9fe1ba3 108 {
yasunorihirakawa 1:bb85c9fe1ba3 109 const float pi = 3.14159265359;
yasunorihirakawa 1:bb85c9fe1ba3 110 const float gear_ratio = 1/38.2;
yasunorihirakawa 1:bb85c9fe1ba3 111 const float tire_size = 57.0/2;
yasunorihirakawa 1:bb85c9fe1ba3 112
yasunorihirakawa 1:bb85c9fe1ba3 113 static float v_pre = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 114 static float dist_count = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 115
yasunorihirakawa 1:bb85c9fe1ba3 116 float vl = motorL.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
yasunorihirakawa 1:bb85c9fe1ba3 117 float vr = motorR.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
yasunorihirakawa 1:bb85c9fe1ba3 118 float v = (vl + vr)/2;
yasunorihirakawa 1:bb85c9fe1ba3 119
yasunorihirakawa 1:bb85c9fe1ba3 120 dist += ( v + v_pre ) * INTVAL_MOTOR/2;
yasunorihirakawa 1:bb85c9fe1ba3 121 dist_count += ( v + v_pre ) * INTVAL_MOTOR/2;
yasunorihirakawa 1:bb85c9fe1ba3 122 v_pre = v;
yasunorihirakawa 1:bb85c9fe1ba3 123
yasunorihirakawa 2:09ea66e396c1 124 if (dist<=2000) standard_rpm = 2000;
yasunorihirakawa 2:09ea66e396c1 125 else if(dist<=2400) standard_rpm = 1200;
yasunorihirakawa 2:09ea66e396c1 126 else if(dist<=4400) standard_rpm = 2000;
yasunorihirakawa 2:09ea66e396c1 127 else standard_rpm = 1000;
yasunorihirakawa 2:09ea66e396c1 128
yasunorihirakawa 1:bb85c9fe1ba3 129 printf("%f\n", dist);
yasunorihirakawa 1:bb85c9fe1ba3 130
yasunorihirakawa 1:bb85c9fe1ba3 131 if( dist_count >= 200 )
yasunorihirakawa 1:bb85c9fe1ba3 132 {
yasunorihirakawa 1:bb85c9fe1ba3 133 LED = !LED;
yasunorihirakawa 1:bb85c9fe1ba3 134 dist_count -= 200;
Arima 0:532ef32974cf 135 }
yasunorihirakawa 1:bb85c9fe1ba3 136
yasunorihirakawa 1:bb85c9fe1ba3 137 if(dist>2147483000)
yasunorihirakawa 1:bb85c9fe1ba3 138 {
yasunorihirakawa 1:bb85c9fe1ba3 139 dist = 0.0;
yasunorihirakawa 1:bb85c9fe1ba3 140 }
yasunorihirakawa 1:bb85c9fe1ba3 141 }
yasunorihirakawa 1:bb85c9fe1ba3 142
yasunorihirakawa 1:bb85c9fe1ba3 143 void control_motor()
yasunorihirakawa 1:bb85c9fe1ba3 144 {
yasunorihirakawa 1:bb85c9fe1ba3 145 while(1)
yasunorihirakawa 1:bb85c9fe1ba3 146 {
yasunorihirakawa 1:bb85c9fe1ba3 147 motorL.drive();
yasunorihirakawa 1:bb85c9fe1ba3 148 motorR.drive();
yasunorihirakawa 1:bb85c9fe1ba3 149 odometry();
yasunorihirakawa 1:bb85c9fe1ba3 150 wait(INTVAL_MOTOR);
yasunorihirakawa 1:bb85c9fe1ba3 151 }
yasunorihirakawa 1:bb85c9fe1ba3 152 }
yasunorihirakawa 1:bb85c9fe1ba3 153
yasunorihirakawa 1:bb85c9fe1ba3 154 int main()
yasunorihirakawa 1:bb85c9fe1ba3 155 {
yasunorihirakawa 1:bb85c9fe1ba3 156 motor_mode = 1;
yasunorihirakawa 1:bb85c9fe1ba3 157 LED = 1;
yasunorihirakawa 1:bb85c9fe1ba3 158
yasunorihirakawa 1:bb85c9fe1ba3 159 pg1.rise(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 160 pg1.fall(&count_pg1);
yasunorihirakawa 1:bb85c9fe1ba3 161 pg2.rise(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 162 pg2.fall(&count_pg2);
yasunorihirakawa 1:bb85c9fe1ba3 163
yasunorihirakawa 1:bb85c9fe1ba3 164 thread_trace.start(callback(line_trace));
yasunorihirakawa 1:bb85c9fe1ba3 165 thread_motor.start(callback(control_motor));
Arima 0:532ef32974cf 166 }