Diff: main.cpp

Branch:

mpu????????

Revision:

44:624a4469ae21

Parent:

43:9a57cec43257

Child:

45:e3df49d7b912

--- a/main.cpp	Sun Mar 19 15:18:39 2017 +0000
+++ b/main.cpp	Tue Mar 21 23:24:04 2017 +0000
@@ -8,7 +8,7 @@
 #define TO_SEND_DATAS_NUM 7
 #define INIT_SERVO_PERIOD_MS 20
 #define WAIT_LOOP_TIME 0.02
-#define CONTROL_VALUES_NUM sizeof(float) + 1
+#define CONTROL_VALUES_NUM sizeof(int) + 2
 #define TO_SEND_CAN_ID 100
 #define SEND_DATAS_LOOP_TIME 0.1
 #define RECEIVE_DATAS_LOOP_TIME 0.1
@@ -18,17 +18,17 @@
 
 #define ERURON_MOVE_DEG_INI_R 1.0
 #define DRUG_MOVE_DEG_INI_R 0.32
-#define ERURON_TRIM_INI_R 0
+#define ERURON_TRIM_INI_R 0.35
 #define DRUG_TRIM_INI_R 0.62
 
 #define ERURON_MOVE_DEG_INI_L 0.8
-#define DRUG_MOVE_DEG_INI_L 0.32
-#define ERURON_TRIM_INI_L 0.3
+#define DRUG_MOVE_DEG_INI_L -0.32
+#define ERURON_TRIM_INI_L 0.4
 #define DRUG_TRIM_INI_L 0.62
 
 #define ERURON_MOVE_RANGE 0.24
 
-#define PHASE_NUM 6.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
+#define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
 
 /*ドラッグラダー
 初期値 0.65
@@ -49,7 +49,7 @@
 DigitalIn setTrimPin(p12);
 DigitalIn EDstatePin(p14);
 DigitalIn setMaxDegPin(p15);
-DigitalOut debugLED(LED2);
+DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 Ticker sendDatasTicker;
@@ -68,7 +68,7 @@
 float drugTrim;
 float eruronMoveDeg;
 float drugMoveDeg;
-int eruronint;
+int eruronint = 1;
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
@@ -229,6 +229,7 @@
             controlValues[i] = recmsg.data[i];
             if(i<sizeof(int)) intvalues[i] = controlValues[i];
         }
+        //   intvalues[0] = controlValues[0];
         eruronint = *(const int *)intvalues;
         led1 = !led1;
     }
@@ -248,16 +249,16 @@
 
 void WriteServo()
 {
-    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0));
+    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)]));
     eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * ((1.0/PHASE_NUM) * eruronint) * ERURON_MOVE_RANGE));
-    pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
+    pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint* ERURON_MOVE_RANGE));
     pc.printf("drValue::%f   ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],eruronint);
     //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
 }
 
 void setTrim()
 {
-    debugLED =  1;
+    led2 =  1;
     if(EDstatePin) {
         eruronTrim = eruronAna.read();
     } else {
@@ -309,7 +310,7 @@
         ////  pc.printf("eMD:%f   dMD:%f    ",eruronMoveDeg,drugMoveDeg);
         //  pc.printf("pitch:%d      roll:%d     yaw:%d\n\r",pitch,roll,yaw);
         led4 = 0;
-        debugLED = 0;
+        led2 = 0;
         //receiveDatas();
 //        sendDatas();
         WriteServo();