Diff: main.cpp

Branch:

XBus???

Revision:

36:ddf4aa818e88

Parent:

35:707119a675dc

Child:

37:1f71ca1e5dd1

--- a/main.cpp	Sat Mar 11 07:46:44 2017 +0000
+++ b/main.cpp	Sat Mar 11 11:06:20 2017 +0000
@@ -20,13 +20,15 @@
 #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
+#define DRUG_TRIM_INI_L 0
 
+/*
 #define kMaxServoNum        1       // 1 - 50
 #define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
 #define kMotionInterval     10      // flame / sec
 #define kMotionMinMark      0x1249
 #define kMotionEndMark      0xED86
+*/
 
 CAN can(p30,p29);
 CANMessage recmsg;
@@ -71,9 +73,7 @@
 void receiveDatas();
 void WriteServo();
 
-static const uint8_t        servoChannel = 0x01;
-
-XBusServo gXBus(p13, NC, NC, kMaxServoNum);
+//XBusServo gXBus(p13, NC, NC, kMaxServoNum);
 Ticker gTimer;
 
 bool servoInit()
@@ -103,6 +103,7 @@
     return true;
 }
 
+/*
 XBusError initXBus()
 {
     XBusError result;
@@ -118,6 +119,7 @@
     }
     return kXBusError_NoError;
 }
+*/
 
 void init()
 {
@@ -139,7 +141,7 @@
     // toStringTicker.attach(&toString,0.5);
     receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
 
-    initXBus();
+    //  initXBus();
 }
 
 void updateDatas()
@@ -177,15 +179,15 @@
 
 float SampleFloat(float f)  //小数点以下第二位を切り捨て
 {
-    int temp = ((f + 0.05) * 100.0) / 5;
+    int temp = ((f + 0.025) * 100.0) / 5;
     float result = temp / 20.0;
     return result;
 }
 
 void WriteServo()
 {
-    drugServo.pulsewidth(calcPulse(/* drugTrim + drugMoveDeg * */(float)controlValues[sizeof(float)] / 2.0));
-    eruronServo.pulsewidth(calcPulse(/* eruronTrim + eruronMoveDeg **/SampleFloat( (eruronfloat - 1.5) * 2.0)));
+    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * float)controlValues[sizeof(float)] / 2.0));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg *SampleFloat( (eruronfloat / 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));
 }
@@ -223,7 +225,6 @@
 int main()
 {
     init();
-    XBusError result;
 
     setTrimPin.mode(PullDown);
     setMaxDegPin.mode(PullDown);
@@ -241,7 +242,6 @@
             setMaxDeg();
         }
         led4 = 0;
-
         debugLED = 0;
         //receiveDatas();
 //        sendDatas();