Diff: main.cpp

Revision:

44:d433bb5f77c0

Parent:

43:5da6b1574227

Child:

47:949e6c2e69fc

--- a/main.cpp	Thu Nov 02 01:56:46 2017 +0000
+++ b/main.cpp	Sat Dec 02 10:52:00 2017 +0000
@@ -50,6 +50,12 @@
 #define CANID_MOTORSPEED    0xF7
 #define CANID_VEHICLESPEED  0xF8
 #define CANID_DISTRQ        0xF9
+#define CANID_RR_RPS        0xFA
+#define CANID_RL_RPS        0xFB
+#define CANID_FR_RPS        0xFC
+#define CANID_FL_RPS        0xFD
+#define CANID_APS_P         0xFE
+#define CANID_APS_S         0xFF
 //++++++++++++++++++++++++++
 
 //++++++++++++++++++++++++++
@@ -62,6 +68,8 @@
 #define CANDATANUM_MOTORSPEED   4
 #define CANDATANUM_VEHICLESPEED 2
 #define CANDATANUM_DISTRQ       2
+#define CANDATANUM_RPS          2
+#define CANDATANUM_APS          2
 //++++++++++++++++++++++++++
 
 void writeCanBuff(long long data, char* buff, int byteNum)
@@ -107,9 +115,15 @@
 //    char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
 //    char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
     char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
-//    char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
+    char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
+    char canRR_RPS[CANDATANUM_RPS]= {0};
+    char canRL_RPS[CANDATANUM_RPS]= {0};
+    char canFR_RPS[CANDATANUM_RPS]= {0};
+    char canFL_RPS[CANDATANUM_RPS]= {0};
+    char canAPS_P[CANDATANUM_APS]= {0};
+    char canAPS_S[CANDATANUM_APS]= {0};
 
-    int canWriteSucceeded = 0;
+    unsigned int canWriteSucceeded = 0;
 
     //wait(1);  //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり
 
@@ -148,20 +162,46 @@
         writeCanBuff(getMotorTorque(RR_MOTOR), canMotorTorqueR, CANDATANUM_MOTORTRQ);
         writeCanBuff(getMotorTorque(RL_MOTOR), canMotorTorqueL, CANDATANUM_MOTORTRQ);
 
+        writeCanBuff(getWheelRps(FR_WHEEL) / LSB_RPS, canFR_RPS, CANDATANUM_RPS);
+        writeCanBuff(getWheelRps(FL_WHEEL) / LSB_RPS, canFL_RPS, CANDATANUM_RPS);
+        writeCanBuff(getWheelRps(RR_MOTOR) / LSB_RPS, canRR_RPS, CANDATANUM_RPS);
+        writeCanBuff(getWheelRps(RL_MOTOR) / LSB_RPS, canRL_RPS, CANDATANUM_RPS);
+
+        writeCanBuff(getRawSensor(APS_PRIMARY), canAPS_P, CANDATANUM_APS);
+        writeCanBuff(getRawSensor(APS_SECONDARY), canAPS_S, CANDATANUM_APS);
+        
+        printf("%d %d %d %d %d\r\n", getMotorTorque(RR_MOTOR), getMotorTorque(RL_MOTOR), calcRequestTorque(), 0, 4000);
+
+//        writeCanBuff((int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f), canTorqueDistribution, CANDATANUM_DISTRQ);
+
         do {
-            if(canWriteSucceeded != 0b11111) {
-                if(0 == (canWriteSucceeded & 0b00001))
-                    canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
-                else if(0 == (canWriteSucceeded & 0b00010))
-                    canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
-                else if(0 == (canWriteSucceeded & 0b00100))
-                    canWriteSucceeded |= (can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ)) << 2);
-                else if(0 == (canWriteSucceeded & 0b01000))
-                    canWriteSucceeded |= (can.write(CANMessage(CANID_RIGHT_TRQ, canMotorTorqueR, CANDATANUM_MOTORTRQ)) << 3);
-                else if(0 == (canWriteSucceeded & 0b10000))
-                    canWriteSucceeded |= (can.write(CANMessage(CANID_LEFT_TRQ, canMotorTorqueL, CANDATANUM_MOTORTRQ)) << 4);
-                //printf("%d\r\n", canWriteSucceeded);
-            }
+            if(0 == (canWriteSucceeded & 0b1))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
+            if(0 == (canWriteSucceeded & 0b10))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
+            if(0 == (canWriteSucceeded & 0b100))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ)) << 2);
+            if(0 == (canWriteSucceeded & 0b1000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_RIGHT_TRQ, canMotorTorqueR, CANDATANUM_MOTORTRQ)) << 3);
+            if(0 == (canWriteSucceeded & 0b10000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_LEFT_TRQ, canMotorTorqueL, CANDATANUM_MOTORTRQ)) << 4);
+
+            if(0 == (canWriteSucceeded & 0b100000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_FR_RPS, canFR_RPS, CANDATANUM_RPS)) << 5);
+            if(0 == (canWriteSucceeded & 0b1000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_FL_RPS, canFL_RPS, CANDATANUM_RPS)) << 6);
+            if(0 == (canWriteSucceeded & 0b10000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_RR_RPS, canRR_RPS, CANDATANUM_RPS)) << 7);
+            if(0 == (canWriteSucceeded & 0b100000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_RL_RPS, canRL_RPS, CANDATANUM_RPS)) << 8);
+            if(0 == (canWriteSucceeded & 0b1000000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_DISTRQ, canTorqueDistribution, CANDATANUM_DISTRQ)) << 9);
+
+            if(0 == (canWriteSucceeded & 0b10000000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_APS_P, canAPS_P, CANDATANUM_APS)) << 10);
+            if(0 == (canWriteSucceeded & 0b100000000000))
+                canWriteSucceeded |= (can.write(CANMessage(CANID_APS_S, canAPS_S, CANDATANUM_APS)) << 11);
+            //printf("%d\r\n", canWriteSucceeded);
         } while(timer.read() < CONTROL_CYCLE_S);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
         //printf("\r\n");