Diff: main.cpp

Revision:

38:11753ee9734f

Parent:

37:ba10cf09c151

Child:

39:c05074379713

--- a/main.cpp	Wed Aug 30 10:18:23 2017 +0000
+++ b/main.cpp	Tue Oct 24 10:19:51 2017 +0000
@@ -43,19 +43,20 @@
 //CAN ID
 #define CANID_BRAKE         0xF1
 #define CANID_REQTRQ        0xF2
-#define CANID_MOTORTRQ      0xF3
-#define CANID_STEERING      0xF4
-#define CANID_WHEELSPEED    0xF5
-#define CANID_MOTORSPEED    0xF6
-#define CANID_VEHICLESPEED  0xF7
-#define CANID_DISTRQ        0xF8
+#define CANID_RIGHT_TRQ     0xF3
+#define CANID_LEFT_TRQ      0xF4
+#define CANID_STEERING      0xF5
+#define CANID_WHEELSPEED    0xF6
+#define CANID_MOTORSPEED    0xF7
+#define CANID_VEHICLESPEED  0xF8
+#define CANID_DISTRQ        0xF9
 //++++++++++++++++++++++++++
 
 //++++++++++++++++++++++++++
 //CANデータのバイト数
 #define CANDATANUM_BRAKE        1
 #define CANDATANUM_REQTRQ       4
-#define CANDATANUM_MOTORTRQ     8
+#define CANDATANUM_MOTORTRQ     4
 #define CANDATANUM_STEERING     1
 #define CANDATANUM_WHEELSPEED   4
 #define CANDATANUM_MOTORSPEED   4
@@ -100,7 +101,8 @@
 {
     char canBrake[CANDATANUM_BRAKE]= {0};
     char canRequestTorque[CANDATANUM_REQTRQ]= {0};
-    char canMotorTorque[CANDATANUM_MOTORTRQ]= {0};
+    char canMotorTorqueR[CANDATANUM_MOTORTRQ]= {0};
+    char canMotorTorqueL[CANDATANUM_MOTORTRQ]= {0};
     char canSteer[CANDATANUM_STEERING]= {0};
     char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
     char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
@@ -143,26 +145,29 @@
         writeCanBuff((int)(getVelocity() / LSB_MOTORSPEED),canVehicleSpeed,CANDATANUM_VEHICLESPEED);
         writeCanBuff(getSteerAngle()+127, canSteer, CANDATANUM_STEERING);
         writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
-        writeCanBuff((0xffffffff & getMotorTorque(RIGHT)) | ((long long)getMotorTorque(LEFT) << 32), canMotorTorque, CANDATANUM_MOTORTRQ);
+        writeCanBuff(getMotorTorque(RIGHT), canMotorTorqueR, CANDATANUM_MOTORTRQ);
+        writeCanBuff(getMotorTorque(LEFT), canMotorTorqueL, CANDATANUM_MOTORTRQ);
 
-        printf("%d\r\n", getSteerAngle());
+//        printf("%d\r\n", getSteerAngle());
 
         do {
-            if(canWriteSucceeded != 0b1111) {
-                if(0 == (canWriteSucceeded & 0b0001))
+            if(canWriteSucceeded != 0b11111) {
+                if(0 == (canWriteSucceeded & 0b00001))
                     canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
-                else if(0 == (canWriteSucceeded & 0b0010))
+                else if(0 == (canWriteSucceeded & 0b00010))
                     canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
-                else if(0 == (canWriteSucceeded & 0b0100))
+                else if(0 == (canWriteSucceeded & 0b00100))
                     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);
+                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);
             }
         } 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));
+//        printf("apsP:%1.2f, apsS:%1.2f, brake:%1.2f,RPS_R:%1.2f, RPS_L%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE), getRPS(RIGHT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f, getRPS(LEFT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f);
     }
 }