Diff: main.cpp

Revision:

34:594ddb4008b2

Parent:

32:9688c30ac38b

Child:

35:b75595b1da36

--- a/main.cpp	Thu Jul 27 15:37:59 2017 +0000
+++ b/main.cpp	Fri Aug 04 02:58:51 2017 +0000
@@ -63,7 +63,7 @@
 #define CANDATANUM_DISTRQ       2
 //++++++++++++++++++++++++++
 
-void writeCanBuff(int data, char* buff, int byteNum)
+void writeCanBuff(long long data, char* buff, int byteNum)
 {
     for(int i=0; i<byteNum; i++) {
         buff[i] = 0xff & (data >> (i*8));
@@ -107,7 +107,7 @@
     char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
     char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
 
-    int INPN_test = -1733;
+    int canWriteSucceeded = 0;
 
     //wait(1);  //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり
 
@@ -124,13 +124,14 @@
 
     timer.start();
 
-    float time;
-
     struct errCounter_t eCounter= {0,0,0,0,0,0,0,0};
 
     wait(3);
 
     while(1) {
+
+        canWriteSucceeded = 0;
+
         getCurrentErrCount(&eCounter);
 
         timer.reset();
@@ -140,14 +141,26 @@
         brakeSignal.write(isBrakeOn());
 
         writeCanBuff((int)(getVelocity() / LSB_MOTORSPEED),canVehicleSpeed,CANDATANUM_VEHICLESPEED);
-        can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED));
         writeCanBuff(getSteerAngle()+127, canSteer, CANDATANUM_STEERING);
-        can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING));
         writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
-        can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ));
+        writeCanBuff((0xffffffff & getMotorTorque(RIGHT)) | ((long long)getMotorTorque(LEFT) << 32), canMotorTorque, CANDATANUM_MOTORTRQ);
+
+        //printf("%d\r\n", calcRequestTorque());
 
-        time = timer.read();
-        while(timer.read_ms() < CONTROL_CYCLE_MS);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
+        do {
+            if(canWriteSucceeded != 0b1111) {
+                if(0 == (canWriteSucceeded & 0b0001))
+                    canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
+                else if(0 == (canWriteSucceeded & 0b0010))
+                    canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
+                else if(0 == (canWriteSucceeded & 0b0100))
+                    canWriteSucceeded |= (can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ)) << 2);
+                else if(0 == (canWriteSucceeded & 0b1000))
+                    canWriteSucceeded |= (can.write(CANMessage(CANID_MOTORTRQ, canMotorTorque, CANDATANUM_MOTORTRQ)) << 3);
+                //printf("%d\r\n", canWriteSucceeded);
+            }
+        } while(timer.read_ms() < CONTROL_CYCLE_MS);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
+        //printf("\r\n");
 
         //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\r\n", eCounter.apsUnderVolt, eCounter.apsExceedVolt, eCounter.apsErrorTolerance, eCounter.apsStick, eCounter.brakeUnderVolt, eCounter.brakeExceedVolt, eCounter.brakeFuzzyVolt, eCounter.brakeOverRide);
         //printf("apsP:%1.2f, apsS:%1.2f, brake:%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE));