Diff: main.cpp

Revision:

2:b7d4195e0fea

Parent:

1:a003b900b810

Child:

3:b79aa7d836fb

diff -r a003b900b810 -r b7d4195e0fea main.cpp
--- a/main.cpp	Fri Jun 22 16:37:21 2018 +0000
+++ b/main.cpp	Sat Jun 23 09:10:07 2018 +0000
@@ -3,18 +3,29 @@
 Serial bc(PA_9,PA_10);              // USART1 Bluetooth (Tx, Rx)
 Serial pc(PA_2,PA_3);               // USART2 pc (Tx, Rx)
 
+InterruptIn up(PC_8);         // Burton Interrupt Motor control
+InterruptIn down(PC_9);
+
+
 DigitalOut led(LED2);               // LED 
 DigitalOut enable(PB_12);           // Motor enable
 DigitalOut p1(PB_13);               // Motor dir1 up
 DigitalOut p2(PB_14);               // Motor dir2 down
 
-DigitalIn up(PC_8);                 // Burton Motor control
-DigitalIn down(PC_9);
+AnalogIn DisSensor(PA_7);           // Distance Sensor
 
-AnalogIn disSensor(PA_7);           // Distance Sensor
+AnalogIn Force_L1(PB_0);            // Force Sensor
+AnalogIn Force_L2(PB_1);
+AnalogIn Force_L3(PC_0);            // Force Sensor
+AnalogIn Force_L4(PC_1);
+AnalogIn Force_R1(PC_2);            // Force Sensor
+AnalogIn Force_R2(PC_3);
+AnalogIn Force_R3(PC_4);            // Force Sensor
+AnalogIn Force_R4(PC_5);
+
 
 Ticker timer1;                      // Control Angle By Using Distance Sensor
-Ticker timer2;                      // Burton Control
+Ticker timer2;                      // Force Sensor
 
 /*------------------ ReadData Variables -------------------*/
 
@@ -113,9 +124,9 @@
 void ControlAng()
 {
         
-        double d1 = disSensor.read();
-        double d2 = 47.512*d1*d1*d1*d1-180.65*d1*d1*d1+240.5*d1*d1-144.03*d1+35.556;    
-        double ref_d = targetDis-d2;
+        float d1 = DisSensor.read();
+        float d2 = 47.512*d1*d1*d1*d1-180.65*d1*d1*d1+240.5*d1*d1-144.03*d1+35.556;    
+        float ref_d = targetDis-d2;
         
 //        bc.printf("%1.3f %1.3f \n", d1, ref_d);
         
@@ -157,7 +168,6 @@
             case 0 : //stop
         
                 enable = 0;
-                timer1.detach();
         
                 break;
         
@@ -191,6 +201,14 @@
     
 }
 
+void ReadForce()
+{
+    
+    float a = Force_L1.read();
+    float b = Force_L2.read();
+    bc.printf(" %1.3f, %1.3f\n", a,b);
+}
+
 
 int main()
 {
@@ -200,15 +218,21 @@
     up.mode(PullNone);
     down.mode(PullNone);
     
+    up.fall(&MotorBurton);                  // 1 > 0 swtich on 
+    up.rise(&MotorBurton);                  // 0 > 1 switch on
+    down.fall(&MotorBurton);
+    down.rise(&MotorBurton);
+    
     int modeLED;
     int modeMotor;
     
     modeLED = 0;
     modeMotor = 0;
+    timer2.attach(&ReadForce,0.05);
     
     while(1)
     {
-         
+    
         bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
         
         // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //       
@@ -235,21 +259,9 @@
                                 
             }
             
-            else if (0 == strcmp(RX_command,"BUR"))
-            {               
-                
-                bc.printf("MOTOR BURTON CONTROL MODE!!");
-                timer1.detach();
-                timer2.attach(&MotorBurton,0.1);
-                                
-            }
-            
-
-            
-            else if (0 == strcmp(RX_command,"DIS"))
+            else if (0 == strcmp(RX_command,"POS"))
             {
-                bc.printf("MOTOR DISTANCE CONTROL MODE!!");
-                timer2.detach();           
+                bc.printf("MOTOR DISTANCE CONTROL MODE!!");          
                 targetDis = atoi(RX_data);
                 timer1.attach(&ControlAng,0.1);
                             
@@ -257,8 +269,7 @@
                    
                     
         }    
-        
-        wait_ms(100);     
+           
         
     }