Diff: main.cpp

Revision:

1:bb85c9fe1ba3

Parent:

0:532ef32974cf

Child:

2:09ea66e396c1

diff -r 532ef32974cf -r bb85c9fe1ba3 main.cpp
--- a/main.cpp	Thu Oct 24 02:22:43 2019 +0000
+++ b/main.cpp	Thu Nov 28 07:21:36 2019 +0000
@@ -1,9 +1,160 @@
 #include "mbed.h"
+#include "motor.h"
+
+DigitalOut  motor_mode(D9);
+DigitalOut  LED(A6);
+Motor       motorL(D5, D6);
+Motor       motorR(D3, D4);
+InterruptIn pg1(D12);
+InterruptIn pg2(D11);
+AnalogIn    reflectorF(A2);
+AnalogIn    reflectorL(A1);
+AnalogIn    reflectorR(A0);
+
+Thread thread_trace;
+Thread thread_motor;
 
-// main() runs in its own thread in the OS
-int main()
+const int   THREAD_FLAG_TRACE(1);
+const int   THREAD_FLAG_MOTOR(2);
+const float INTVAL_REFLECTOR (0.01);
+const float INTVAL_MOTOR     (0.25);
+
+float dist = 0.0;
+
+void count_pg1()
+{
+    motorL.count();
+}
+
+void count_pg2()
+{
+    motorR.count();
+} 
+
+void line_trace()
 {
-    while (true) {
+    const float fast_rpm = 1500;
+    const float standard_rpm = 1200;
+    const float slow_rpm = 750;
+    const float K = 850;
+    const int   L = 0;
+    const int   R = 1;
+    
+    int flag;
+    
+    motorL.Set_phase(0);
+    motorR.Set_phase(1);
+    
+    while(1)
+    {
+        if( reflectorF<=0.4 && reflectorR<=0.4 && reflectorL<= 0.4 )
+        {      
+            if( flag == L )
+            {
+               motorL.Set_phase(1);
+               motorL.Set_target(slow_rpm);
+               motorR.Set_phase(1);
+               motorR.Set_target(slow_rpm);
+               //printf("turnL\n");
+            }
+            else
+            {
+               motorL.Set_phase(0);
+               motorL.Set_target(slow_rpm);
+               motorR.Set_phase(0);
+               motorR.Set_target(slow_rpm);
+               //printf("turnR\n");
+            }  
+        }
+        else
+        {
+            if( reflectorF > 0.4 )
+            {
+                motorL.Set_phase(0);
+                motorL.Set_target(fast_rpm);
+                motorR.Set_phase(1);
+                motorR.Set_target(fast_rpm);
+            }
+            else
+            {
+                if( reflectorL.read() >= reflectorR.read() )
+                {
+                    float voltage_gap = reflectorL.read() - reflectorR.read();
+                    motorR.Set_phase(1);
+                    motorR.Set_target(standard_rpm + K * voltage_gap);
+                    motorL.Set_phase(0);
+                    motorL.Set_target(standard_rpm - K * voltage_gap);
+                    flag = L;
+                }
+                else
+                {
+                    float voltage_gap = reflectorR.read() - reflectorL.read();
+                    motorR.Set_phase(1);
+                    motorR.Set_target(standard_rpm - K * voltage_gap);
+                    motorL.Set_phase(0); 
+                    motorL.Set_target(standard_rpm + K * voltage_gap);
+                    flag = R;
+                }
+            }
+            wait(INTVAL_REFLECTOR);
+        }
+        
+        //printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
+    }
+}
 
+void odometry()
+{
+    const float pi         = 3.14159265359;
+    const float gear_ratio = 1/38.2;
+    const float tire_size  = 57.0/2;
+    
+    static float v_pre      = 0.0;
+    static float dist_count = 0.0;
+
+    float vl = motorL.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
+    float vr = motorR.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
+    float v  = (vl + vr)/2;
+    
+    dist       += ( v + v_pre ) * INTVAL_MOTOR/2;
+    dist_count += ( v + v_pre ) * INTVAL_MOTOR/2;
+    v_pre = v;
+    
+    printf("%f\n", dist);
+    
+    if( dist_count >= 200 )
+    {
+        LED = !LED;
+        dist_count -= 200;
     }
+    
+    if(dist>2147483000)
+    {
+        dist = 0.0;
+    }
+}
+
+void control_motor()
+{
+    while(1)
+    {
+        motorL.drive();
+        motorR.drive();
+        odometry();
+        wait(INTVAL_MOTOR);
+    }
+}   
+
+int main()
+{   
+    motor_mode = 1;
+    LED = 1;
+
+    pg1.rise(&count_pg1);
+    pg1.fall(&count_pg1);
+    pg2.rise(&count_pg2);
+    pg2.fall(&count_pg2);
+    
+    thread_trace.start(callback(line_trace));
+    thread_motor.start(callback(control_motor));
 }
\ No newline at end of file