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@1:bb85c9fe1ba3, 2019-11-28 (annotated)

Committer:
yasunorihirakawa
Date:
Thu Nov 28 07:21:36 2019 +0000
Revision:
1:bb85c9fe1ba3
Parent:
0:532ef32974cf
Child:
2:09ea66e396c1
a

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