Diff: main.cpp

Branch:

XBus???

Revision:

35:707119a675dc

Parent:

33:d075918d4846

Child:

36:ddf4aa818e88

--- a/main.cpp	Sat Mar 11 05:28:36 2017 +0000
+++ b/main.cpp	Sat Mar 11 07:46:44 2017 +0000
@@ -12,13 +12,13 @@
 #define SEND_DATAS_LOOP_TIME 0.1
 #define RECEIVE_DATAS_LOOP_TIME 0.1
 
-#define ERURON_MOVE_DEG_INI_R 0
-#define DRUG_MOVE_DEG_INI_R 0
+#define ERURON_MOVE_DEG_INI_R 1.0
+#define DRUG_MOVE_DEG_INI_R 1.0
 #define ERURON_TRIM_INI_R 0
 #define DRUG_TRIM_INI_R 0
 
-#define ERURON_MOVE_DEG_INI_L 0
-#define DRUG_MOVE_DEG_INI_L 30
+#define ERURON_MOVE_DEG_INI_L 1.0
+#define DRUG_MOVE_DEG_INI_L 1.0
 #define ERURON_TRIM_INI_L 0
 #define DRUG_TRIM_INI_L 90
 
@@ -170,24 +170,24 @@
     }
 }
 
-double calcPulse(int deg)
+double calcPulse(float deg)
 {
-    return (0.0006+(deg/180.0)*(0.00235-0.00045));
+    return (0.0006+(deg)*(0.00235-0.00045));
 }
 
-void XbusIntervalHandler()
+float SampleFloat(float f)  //小数点以下第二位を切り捨て
 {
-    uint16_t diff  = kMotionEndMark - kMotionMinMark;
-    XbusValue = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * eruronfloat / 2.0) + kMotionMinMark);
-    //pc.printf("%f",value);
-    gXBus.setServo(servoChannel, XbusValue);
-    gXBus.sendChannelDataPacket();
+    int temp = ((f + 0.05) * 100.0) / 5;
+    float result = temp / 20.0;
+    return result;
 }
 
 void WriteServo()
 {
-    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[sizeof(float)]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat ));
+    drugServo.pulsewidth(calcPulse(/* drugTrim + drugMoveDeg * */(float)controlValues[sizeof(float)] / 2.0));
+    eruronServo.pulsewidth(calcPulse(/* eruronTrim + eruronMoveDeg **/SampleFloat( (eruronfloat - 1.5) * 2.0)));
+    // pc.printf("dr::%f   ef::%f\n\r",(float)controlValues[sizeof(float)],eruronfloat - 1.5);
+    pc.printf("raw:%f    sampled:%f\n\r",(eruronfloat - 1.5) * 2.0,SampleFloat( (eruronfloat - 1.5) * 2.0));
 }
 
 void setTrim()
@@ -215,7 +215,6 @@
         drugServo.pulsewidth(calcPulse(drugTemp));
         drugMoveDeg = drugTemp-drugTrim;
     }
-    // pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
     wait_us(10);
@@ -241,10 +240,6 @@
         while (setMaxDegPin) {
             setMaxDeg();
         }
-        //  pc.printf("eT:%f\n\r",eruronTrim);
-//         pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-         pc.printf("eMD:%f   dMD:%f    ef:%f\n",eruronMoveDeg,drugMoveDeg,eruronfloat);
-        pc.printf("EV:%f,DG:%d\n\r",eruronfloat,controlValues[sizeof(float)]);
         led4 = 0;
 
         debugLED = 0;
@@ -252,8 +247,6 @@
 //        sendDatas();
         WriteServo();
         updateDatas();
-//      pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
-      //  pc.printf("%f\n\r",XbusValue);
         led3 = !led3;
         wait(WAIT_LOOP_TIME);
     }