Diff: main.cpp

Revision:

28:47e9531a3a9d

Parent:

27:37a8b4f6d28d

Child:

29:a51cb2cf22ae

--- a/main.cpp	Sat Jul 01 08:45:37 2017 +0000
+++ b/main.cpp	Mon Jul 03 12:03:34 2017 +0000
@@ -40,14 +40,34 @@
 #define isShutdownSystem(void)  (SDState.read())
 
 //++++++++++++++++++++++++++
+//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
+//++++++++++++++++++++++++++
+
+//++++++++++++++++++++++++++
 //CANデータのバイト数
-#define CANDATANUM_REQTRQ 2
-#define CANDATANUM_BRAKE 1
-#define CANDATANUM_MOTORTRQ_R 2
-#define CANDATANUM_MOTORTRQ_L 2
-#define CANDATANUM_MOTORTRQ_L 1
+#define CANDATANUM_BRAKE        1
+#define CANDATANUM_REQTRQ       4
+#define CANDATANUM_MOTORTRQ     8
+#define CANDATANUM_STEERING     1
+#define CANDATANUM_WHEELSPEED   4
+#define CANDATANUM_MOTORSPEED   4
+#define CANDATANUM_VEHICLESPEED 2
 //++++++++++++++++++++++++++
 
+void writeCanBuff(int data, char* buff, int byteNum)
+{
+    for(int i=0; i<byteNum; i++) {
+        buff[i] = 0xff & (data >> (i*8));
+    }
+}
+
 void init(void)
 {
     indicatorLed = 0;
@@ -76,8 +96,13 @@
 
 int main(void)
 {
-    int f_whatchdog=0;
-    char canSendData[1];
+    char canBrake[CANDATANUM_BRAKE]= {0};
+    char canRequestTorque[CANDATANUM_REQTRQ]= {0};
+    char canMotorTorque[CANDATANUM_MOTORTRQ]= {0};
+    char canSteer[CANDATANUM_STEERING]= {0};
+    char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
+    char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
+    char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
 
     wait(1);
 
@@ -101,8 +126,6 @@
     wait(1);
 
     while(1) {
-        //f_whatchdog = ~f_whatchdog;
-        //WDT.write(f_whatchdog);
         //getCurrentErrCount(&eCounter);
 
         timer.reset();
@@ -111,10 +134,21 @@
 
         //printf("%f\n\r", 45.0/0xFFFF * calcRequestTorque());
 
-        canSendData[0] = (char)(255.0f * (calcRequestTorque()-MAX_OUTPUT_TORQUE_REGENERATIVE)/(MAX_OUTPUT_TORQUE_POWER-MAX_OUTPUT_TORQUE_REGENERATIVE));
-        printf("%2.2f\t%3.2f\t%1.2f\tsend:%d\r\n", 45.0f / 0xffff * calcRequestTorque(), getVelocity(), getSteerAngle(), canSendData[0]);
+        writeCanBuff(isBrakeOn(),                                           canBrake,           CANDATANUM_BRAKE);
+        writeCanBuff(calcRequestTorque(),                                   canRequestTorque,   CANDATANUM_REQTRQ);
+        writeCanBuff(getMotorTorque(RIGHT) + (getMotorTorque(LEFT) << 16),  canMotorTorque,     CANDATANUM_MOTORTRQ);
+        writeCanBuff(getSteerAngle(),                                       canSteer,           CANDATANUM_STEERING);
+        writeCanBuff(getRPS(RIGHT, 0) + (getRPS(LEFT, 0)<<16),              canMotorSpeed,      CANDATANUM_MOTORSPEED);
+        writeCanBuff(getRPS(RIGHT, 1) + (getRPS(LEFT, 1)<<16),              canWheelSpeed,      CANDATANUM_WHEELSPEED);
+        writeCanBuff(getVelocity() / 0.1f,                                  canVehicleSpeed,    CANDATANUM_VEHICLESPEED);
 
-        can.write(CANMessage(0x0f, canSendData, 1));
+        //can.write(CANMessage(CANID_BRAKE, canBrake, CANDATANUM_BRAKE));
+        can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ));
+        can.write(CANMessage(CANID_MOTORTRQ, canMotorTorque, CANDATANUM_MOTORTRQ));
+        can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING));
+        can.write(CANMessage(CANID_WHEELSPEED, canMotorSpeed, CANDATANUM_MOTORSPEED));
+        can.write(CANMessage(CANID_MOTORSPEED, canWheelSpeed, CANDATANUM_WHEELSPEED));
+        can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED));
 
         //printf("%1.6f\r\n", time);
         //printf("%d\r\n", eCounter.brakeOverRide);