Diff: main.cpp

Revision:

12:fd9d241843f4

Parent:

11:44fea1d768b5

Child:

13:5e3b4120dbbf

--- a/main.cpp	Thu Feb 25 09:44:58 2016 +0000
+++ b/main.cpp	Fri Feb 26 09:00:53 2016 +0000
@@ -10,12 +10,16 @@
 #define TO_SEND_CAN_ID 100
 #define ADXL_MEAN_NUM 10
 #define SEND_DATAS_LOOP_TIME 0.1
-#define ERURON_MOVE_DEG_INI 10
-#define DRUG_MOVE_DEG_INI -92
-#define ERURON_TRIM_INI 96
-#define DRUG_TRIM_INI 109
-#define start 510
-#define end 2390
+
+#define ERURON_MOVE_DEG_INI_R 10
+#define DRUG_MOVE_DEG_INI_R -92
+#define ERURON_TRIM_INI_R 96
+#define DRUG_TRIM_INI_R 109
+
+#define ERURON_MOVE_DEG_INI_L 10
+#define DRUG_MOVE_DEG_INI_L -92
+#define ERURON_TRIM_INI_L 96
+#define DRUG_TRIM_INI_L 109
 
 CAN can(p30,p29);
 CANMessage recmsg;
@@ -28,8 +32,9 @@
 DigitalOut led1(LED1);
 AnalogIn drugAna(p20);
 AnalogIn eruronAna(p19);
+DigitalIn LRstatePin(p14);
 DigitalIn setTrimPin(p15);
-DigitalIn debugServoPin(p16);
+DigitalIn EDstatePin(p16);
 DigitalIn checkMaxDegPin(p17);
 DigitalOut debugLED(LED2);
 DigitalOut led3(LED3);
@@ -40,10 +45,10 @@
 char toSendDatas[TO_SEND_DATAS_NUM];
 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
 
-float eruronTrim = ERURON_TRIM_INI;
-float drugTrim = DRUG_TRIM_INI;
-float eruronMoveDeg = ERURON_MOVE_DEG_INI;
-float drugMoveDeg = DRUG_MOVE_DEG_INI;
+float eruronTrim;
+float drugTrim;
+float eruronMoveDeg;
+float drugMoveDeg;
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
@@ -90,6 +95,18 @@
 }
 
 void init(){
+    if(LRstatePin){
+        eruronTrim = ERURON_TRIM_INI_R;
+        drugTrim = DRUG_TRIM_INI_R;
+        eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
+        drugMoveDeg = DRUG_MOVE_DEG_INI_R;
+    }
+    else{
+        eruronTrim = ERURON_TRIM_INI_L;
+        drugTrim = DRUG_TRIM_INI_L;
+        eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
+        drugMoveDeg =DRUG_MOVE_DEG_INI_L;
+    }
     SERVO_FLAG = servoInit();
     ADXL_FLAG = adxlInit();
     INA_FLAG = inaInit();
@@ -133,27 +150,31 @@
 }
 
 void WriteServo(){
-    if(debugServoPin){
-        led3 = 1;
-        float a = eruronAna.read()*180;
-        float b = drugAna.read()*180;
-        eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
-        drugServo.pulsewidth(calcPulse(drugAna.read()*180));
-        pc.printf("eruron:%f    drug:%f\n\r",a,b);
-    }
-    else{
-        led3 = 0;
+    //if(debugServoPin){
+//        led3 = 1;
+//        float a = eruronAna.read()*180;
+//        float b = drugAna.read()*180;
+//        eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
+//        drugServo.pulsewidth(calcPulse(drugAna.read()*180));
+//        pc.printf("eruron:%f    drug:%f\n\r",a,b);
+//    }
+//    else{
+      //  led3 = 0;
         eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1)));
         drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1]));
-    }
+    //}
 }
 
 void setTrim(){
     debugLED =  1;
+    if(EDstatePin){
     eruronTrim = eruronAna.read()*180;
+    eruronServo.pulsewidth(calcPulse(eruronTrim));
+    }
+    else{
     drugTrim = drugAna.read()*180;
-    eruronServo.pulsewidth(calcPulse(eruronTrim));
     drugServo.pulsewidth(calcPulse(drugTrim));
+    }
     pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
 }
 
@@ -161,10 +182,14 @@
     led4 = 1;
     float eruronTemp = eruronAna.read()*180;
     float drugTemp = drugAna.read()*180;
+    if(EDstatePin){
     eruronServo.pulsewidth(calcPulse(eruronTemp));
+    eruronMoveDeg = eruronTemp-eruronTrim;
+    }
+    else{
     drugServo.pulsewidth(calcPulse(drugTemp));
-    eruronMoveDeg = eruronTemp-eruronTrim;
     drugMoveDeg = drugTemp-drugTrim;
+    }
     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
     wait_us(10);
 }