Diff: main.cpp

Revision:

5:7459a428e16e

Parent:

3:4417217b4f66

Child:

7:d6a3e47e6ef9

--- a/main.cpp	Wed Feb 17 01:56:21 2016 +0000
+++ b/main.cpp	Wed Feb 17 02:29:14 2016 +0000
@@ -15,10 +15,11 @@
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
 PwmOut servo1(p22);
-PwmOut servo2(p23);
+PwmOut servo2(p23);// eruron
 DigitalOut led1(LED1);
 AnalogIn a(p20);
 AnalogIn b(p19);
+DigialIn switch1(p15);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
@@ -31,6 +32,8 @@
 bool SERVO_FLAG;
 bool ADXL_FLAG;
 bool INA_FLAG;
+int neutral_a = 0;
+int neutral_b = 0;
 
 int acc[3] = {0,0,0};
 
@@ -122,32 +125,45 @@
     return (0.00093+(deg/180.0)*(0.00235-0.00077));
 }
 
-void WriteServo(){
+void WriteServo(int a,int b){
+    if(a >= 45 && a <= 90 && b >= 45 && b <= 90){
     if(controlValues[0] == (char)1){
-        servo1.pulsewidth(calcPulse(90));
-    } 
+      servo1.Pulsewidth(calcPulse(90));
     else{
-        servo1.pulsewidth(calcPulse(45));
+        servo1.pulsewidth(calcPulse(a));
     }
     if(controlValues[1] == (char)1){
         servo2.pulsewidth(calcPulse(90));
     } 
     else{
-        servo2.pulsewidth(calcPulse(45));
+        servo2.pulsewidth(calcPulse(b));
+    }
     }
     //servo1.pulsewidth(calcPulse(a.read()*170));
 //    servo2.pulsewidth(calcPulse(b.read()*170));
     //pc.printf("%f", a.read());
 }
 
+
+//初期迎角を変える
+void Debug(){
+    if(switch1 ==  1){
+    neutral_a = (int)(a * 180);
+    neutral_b = (int)(b * 180);
+    WriteServo(neutral_a);
+    WriteServo(neutral_b);
+    }
+    }
+    
 int main(){
     init();
     while(1){
         receiveDatas();
-        WriteServo();
+        WriteServo(neutral_a,neutral_b);
            updateDatas();
         toString();
         sendDatas();
+        Debug();
         wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file