Diff: main.cpp

Revision:

3:b79aa7d836fb

Parent:

2:b7d4195e0fea

Child:

4:bf4ad2079096

--- a/main.cpp	Sat Jun 23 09:10:07 2018 +0000
+++ b/main.cpp	Fri Jul 20 02:50:24 2018 +0000
@@ -3,7 +3,7 @@
 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 up(PC_8);         // Button Interrupt Motor control
 InterruptIn down(PC_9);
 
 
@@ -12,18 +12,19 @@
 DigitalOut p1(PB_13);               // Motor dir1 up
 DigitalOut p2(PB_14);               // Motor dir2 down
 
-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);
+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 timer0;                      // Operating MCU LED on 
 Ticker timer1;                      // Control Angle By Using Distance Sensor
 Ticker timer2;                      // Force Sensor
 
@@ -124,13 +125,14 @@
 void ControlAng()
 {
         
-        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 d1 = disSensor.read();
+        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1262.7f*d1+340.25f;
+//        float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1600.0f*d1+340.25f;
         float ref_d = targetDis-d2;
         
-//        bc.printf("%1.3f %1.3f \n", d1, ref_d);
+        bc.printf("%1.3f %1.3f %1.3f\n", d1, d2, ref_d);
         
-        if(ref_d > 0.01)
+        if(ref_d > 0.9f)
         {
                 enable = 1;
                 p1 = 0;
@@ -138,7 +140,7 @@
                 
         }
         
-        else if(ref_d < -0.01)
+        else if(ref_d < -0.9f)
         {
                     
                 enable = 1;
@@ -156,7 +158,7 @@
         }
 }
 
-void MotorBurton()
+void MotorButton()
 {
         
         int a = up.read();
@@ -203,33 +205,52 @@
 
 void ReadForce()
 {
+    float force_L[4];
+    float force_R[4];
     
-    float a = Force_L1.read();
-    float b = Force_L2.read();
-    bc.printf(" %1.3f, %1.3f\n", a,b);
+    force_L[0] = force_L1.read();
+    force_L[1] = force_L2.read();
+    force_L[2] = force_L3.read();
+    force_L[3] = force_L4.read();
+    force_R[0] = force_R1.read();
+    force_R[1] = force_R2.read();
+    force_R[2] = force_R3.read();
+    force_R[3] = force_R4.read();
+    
+//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_L[0],force_L[1],force_L[2],force_L[3]);
+//    bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_R[0],force_R[1],force_R[2],force_R[3]);
 }
 
+void OperateLED()
+{
+    led =!led;
+}
 
 int main()
 {
     bc.baud(9600);
     bc.printf("START 180622 Ver. \n");
+    led = 1;  
     
     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);
+    up.fall(&MotorButton);                  // 1 > 0 swtich on 
+    up.rise(&MotorButton);                  // 0 > 1 switch on
+    down.fall(&MotorButton);
+    down.rise(&MotorButton);
     
     int modeLED;
     int modeMotor;
     
     modeLED = 0;
     modeMotor = 0;
+    
+    timer0.attach(&OperateLED,0.5);
     timer2.attach(&ReadForce,0.05);
     
+    
+    
     while(1)
     {