Diff: main.cpp

Revision:

4:450cafd95ac3

Parent:

3:4417217b4f66

Child:

10:439e73c9a207

--- a/main.cpp	Wed Feb 17 01:56:21 2016 +0000
+++ b/main.cpp	Wed Feb 24 10:21:46 2016 +0000
@@ -2,11 +2,18 @@
 #include "mbed.h"
 #include "ADXL345_I2C.h"
 #include "INA226.hpp"
-#define TO_SEND_DATAS_NUM 5
+
+#define TO_SEND_DATAS_NUM 4
 #define INIT_SERVO_PERIOD_MS 20
-#define WAIT_LOOP_TIME 0.2
+#define WAIT_LOOP_TIME 0.02
 #define CONTROL_VALUES_NUM 2
 #define TO_SEND_CAN_ID 100
+#define ADXL_MEAN_NUM 10
+#define SEND_DATAS_LOOP_TIME 0.1
+#define ERURON_MOVE_DEG_INI 0
+#define DRUG_MOVE_DEG_INI 0
+#define start 510
+#define end 2390
 
 CAN can(p30,p29);
 CANMessage recmsg;
@@ -14,18 +21,27 @@
 ADXL345_I2C accelerometer(p9, p10);
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
-PwmOut servo1(p22);
-PwmOut servo2(p23);
+PwmOut drugServo(p22);
+PwmOut eruronServo(p23);
 DigitalOut led1(LED1);
-AnalogIn a(p20);
-AnalogIn b(p19);
+AnalogIn drugAna(p20);
+AnalogIn eruronAna(p19);
+DigitalIn setTrimPin(p15);
+DigitalIn debugServoPin(p16);
+DigitalIn checkMaxDegPin(p17);
+DigitalOut debugLED(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+Ticker sendDatasTicker;
+Ticker toStringTicker;
 
 char toSendDatas[TO_SEND_DATAS_NUM];
 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
 
-int counter = 0;
-int eruron_deg = 0;
-int drug_deg = 0;
+float eruronTrim;
+float drugTrim;
+float eruronMoveDeg = ERURON_MOVE_DEG_INI;
+float drugMoveDeg = DRUG_MOVE_DEG_INI;
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
@@ -33,10 +49,14 @@
 bool INA_FLAG;
 
 int acc[3] = {0,0,0};
+char acc_mean[3][ADXL_MEAN_NUM];
+int adxl_mean_counter = 0;
+
+void toString();
 
 bool servoInit(){
-    servo1.period_ms(INIT_SERVO_PERIOD_MS);
-    servo2.period_ms(INIT_SERVO_PERIOD_MS);
+    drugServo.period_ms(INIT_SERVO_PERIOD_MS);
+    eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
     return true;
 }
 
@@ -48,6 +68,11 @@
     return true;
 }
 
+void sendDatas(){
+   if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
+    }
+}
+
 bool inaInit(){
     if(!VCmonitor.isExist()){
         pc.printf("VCmonitor NOT FOUND\n");
@@ -63,10 +88,11 @@
 }
 
 void init(){
-   // pc.printf("Receiver\n\r");
     SERVO_FLAG = servoInit();
     ADXL_FLAG = adxlInit();
     INA_FLAG = inaInit();
+    sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
+   // toStringTicker.attach(&toString,0.5);
 }
 
 void updateDatas(){
@@ -79,75 +105,80 @@
     }
     for(int i = 0; i < 3; i++){
         toSendDatas[i] = acc[i];
-        //pc.printf("%d\t",acc[i]);
     }
-    //pc.printf("\n");
     toSendDatas[3] = (char)(V/100);
-    toSendDatas[4] = (char)(C/0100);
 }
 
 void receiveDatas(){
     if(can.read(recmsg)){
         for(int i = 0; i < CONTROL_VALUES_NUM; i++){
             controlValues[i] = recmsg.data[i];
-         //   pc.printf("%d:%d    ",i,recmsg.data[i]);
         }
         led1 = !led1;
     }
 }
 
 void toString(){
-    //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
-    //    pc.printf("%i:",toSendDatas[3]);
-//        pc.printf("%i:",toSendDatas[4]);
-       pc.printf("%f:",V);
-    pc.printf("%f:",C);
-//    //}
-    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
-//      //  pc.printf("%d, ",controlValues[i]);
-//        
-//    }
-    //pc.printf("\n\r");
-//    pc.printf("%f",a.read());
-    //pc.printf("\n\r");
+    for(int i = 0; i <CONTROL_VALUES_NUM; i++){
+        pc.printf("%d, ",controlValues[i]);
+    }
+    pc.printf("\n\r");
 }
 
-void sendDatas(){
-   if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
-    //
-      //  pc.printf("resend suc\n\r");   
-}
-}
 double calcPulse(int deg){
-    return (0.00093+(deg/180.0)*(0.00235-0.00077));
+    return (0.0006+(deg/180.0)*(0.00235-0.00045));
+
 }
 
 void WriteServo(){
-    if(controlValues[0] == (char)1){
-        servo1.pulsewidth(calcPulse(90));
-    } 
+    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));
+    }
     else{
-        servo1.pulsewidth(calcPulse(45));
+        led3 = 0;
+        eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1)));
+        drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1]));
     }
-    if(controlValues[1] == (char)1){
-        servo2.pulsewidth(calcPulse(90));
-    } 
-    else{
-        servo2.pulsewidth(calcPulse(45));
-    }
-    //servo1.pulsewidth(calcPulse(a.read()*170));
-//    servo2.pulsewidth(calcPulse(b.read()*170));
-    //pc.printf("%f", a.read());
+}
+
+void setTrim(){
+    debugLED =  1;
+    eruronTrim = eruronAna.read()*180;
+    drugTrim = drugAna.read()*180;
+    eruronServo.pulsewidth(calcPulse(eruronTrim));
+    drugServo.pulsewidth(calcPulse(drugTrim));
+    pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
+}
+
+void checkMaxDeg(){
+    led4 = 1;
+    float eruronTemp = eruronAna.read()*180;
+    float drugTemp = drugAna.read()*180;
+    eruronServo.pulsewidth(calcPulse(eruronTemp));
+    drugServo.pulsewidth(calcPulse(drugTemp));
+    eruronMoveDeg = eruronTemp-eruronTrim;
+    drugMoveDeg = drugTemp-drugTrim;
+    wait_us(10);
 }
 
 int main(){
     init();
     while(1){
+        while(setTrimPin){
+            setTrim();
+        }
+        while(checkMaxDegPin){
+            checkMaxDeg();
+        }
+        led4 = 0;
+        debugLED = 0;
         receiveDatas();
         WriteServo();
-           updateDatas();
-        toString();
-        sendDatas();
+        updateDatas();
         wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file