Diff: main.cpp

Revision:

1:9cc932a16d17

Parent:

0:e052602db102

Child:

2:7fcb4f970a02

--- a/main.cpp	Fri Jan 29 13:38:29 2016 +0000
+++ b/main.cpp	Sat Feb 13 11:35:50 2016 +0000
@@ -15,9 +15,10 @@
 INA226 VCmonitor(ina226_i2c);
 PwmOut servo1(p21);
 PwmOut servo2(p22);
+DigitalOut led1(LED1);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
-char controlValues[CONTROL_VALUES_NUM];
+char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
 
 int counter = 0;
 int eruron_deg = 0;
@@ -85,6 +86,7 @@
         for(int i = 0; i < recmsg.len; i++){
             controlValues[i] = recmsg.data[i];
         }
+        led1 = !led1;
     }
 }
 
@@ -107,14 +109,25 @@
         pc.printf("resend suc\n\r");   
 }
 
-void 
+double calcPulse(int deg){
+    return (0.00093+(deg/180.0)*(0.00235-0.00077));
+}
+
+void WriteServo(){
+    if(controlValues[0]){
+        servo1.pulsewidth(calcPulse(90));
+    } 
+    else{
+        servo1.pulsewidth(calcPulse(45));
+    }
+}
 
 int main(){
     init();
     while(1){
         receiveDatas();
         updateDatas();
-        //WriteServo();
+        WriteServo();
         toString();
         sendDatas();
         wait_ms(WAIT_LOOP_TIME_MS);