Diff: main.cpp

Branch:

XBus???

Revision:

33:d075918d4846

Parent:

32:b03557a08efa

Child:

35:707119a675dc

--- a/main.cpp	Fri Mar 10 12:59:41 2017 +0000
+++ b/main.cpp	Sat Mar 11 05:28:36 2017 +0000
@@ -10,7 +10,7 @@
 #define CONTROL_VALUES_NUM sizeof(float) + 1
 #define TO_SEND_CAN_ID 100
 #define SEND_DATAS_LOOP_TIME 0.1
-#define RECEIVE_DATAS_LOOP_TIME 0.05
+#define RECEIVE_DATAS_LOOP_TIME 0.1
 
 #define ERURON_MOVE_DEG_INI_R 0
 #define DRUG_MOVE_DEG_INI_R 0
@@ -18,9 +18,9 @@
 #define DRUG_TRIM_INI_R 0
 
 #define ERURON_MOVE_DEG_INI_L 0
-#define DRUG_MOVE_DEG_INI_L 0
+#define DRUG_MOVE_DEG_INI_L 30
 #define ERURON_TRIM_INI_L 0
-#define DRUG_TRIM_INI_L 0
+#define DRUG_TRIM_INI_L 90
 
 #define kMaxServoNum        1       // 1 - 50
 #define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
@@ -35,6 +35,7 @@
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
 PwmOut drugServo(p22);
+PwmOut eruronServo(p23);
 DigitalOut led1(LED1);
 AnalogIn drugAna(p20);
 AnalogIn eruronAna(p19);
@@ -50,7 +51,7 @@
 Ticker receiveDatasTicker;
 
 char toSendDatas[TO_SEND_DATAS_NUM];
-char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug
+char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
 char floatvalues[sizeof(float)];
 float eruronTrim;
 float drugTrim;
@@ -162,7 +163,7 @@
     if(can.read(recmsg)) {
         for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
             controlValues[i] = recmsg.data[i];
-            if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
+            if(i<sizeof(float)) floatvalues[i] = controlValues[i];
         }
         eruronfloat = *(const float *)floatvalues;
         led1 = !led1;
@@ -185,7 +186,8 @@
 
 void WriteServo()
 {
-    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
+    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[sizeof(float)]));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat ));
 }
 
 void setTrim()
@@ -230,7 +232,7 @@
     LRstatePin.mode(PullDown);
 
     // start motion
-    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
+//    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
 
     while(1) {
         while(setTrimPin) {
@@ -240,9 +242,9 @@
             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\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
-       // pc.printf("%c,%c,%c,DG:%c\n\r",controlValues[0],controlValues[1],controlValues[2],controlValues[3]);
+//         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;