Diff: main.cpp

Revision:

31:042c08a7434f

Parent:

30:c596a0f5d685

Child:

32:9688c30ac38b

--- a/main.cpp	Fri Jul 21 10:20:57 2017 +0000
+++ b/main.cpp	Thu Jul 27 12:34:06 2017 +0000
@@ -48,6 +48,7 @@
 #define CANID_WHEELSPEED    0xF5
 #define CANID_MOTORSPEED    0xF6
 #define CANID_VEHICLESPEED  0xF7
+#define CANID_DISTRQ        0xF8
 //++++++++++++++++++++++++++
 
 //++++++++++++++++++++++++++
@@ -59,6 +60,7 @@
 #define CANDATANUM_WHEELSPEED   4
 #define CANDATANUM_MOTORSPEED   4
 #define CANDATANUM_VEHICLESPEED 2
+#define CANDATANUM_DISTRQ       2
 //++++++++++++++++++++++++++
 
 void writeCanBuff(int data, char* buff, int byteNum)
@@ -103,9 +105,12 @@
     char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
     char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
     char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
+    char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
 
-    //wait(1);  //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり 
-    
+    int INPN_test = -1733;
+
+    //wait(1);  //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり
+
     pc.baud(115200);
     printf("\033[2J");          //teratermの画面クリア
     printf("\033[1;1H");        //teratermの画面クリア
@@ -131,32 +136,18 @@
         timer.reset();
 
         driveTVD(0, isPressedRTD());
-        
+
         brakeSignal.write(isBrakeOn());
 
-        /*
-        writeCanBuff(isBrakeOn(),                                           canBrake,           CANDATANUM_BRAKE);
-        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(CANID_BRAKE, canBrake, CANDATANUM_BRAKE));
-        can.write(CANMessage(CANID_MOTORTRQ, canMotorTorque, CANDATANUM_MOTORTRQ));
+        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));
-        can.write(CANMessage(CANID_WHEELSPEED, canMotorSpeed, CANDATANUM_MOTORSPEED));
-        can.write(CANMessage(CANID_MOTORSPEED, canWheelSpeed, CANDATANUM_WHEELSPEED));
-        can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED));
-        */
-        
         writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
         can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ));
-        
-        //printf("%1.6f\r\n", time);
-        
-        printf("%d:%d %d %d %d\r\n", calcRequestTorque(), (int)canRequestTorque[0],(int)canRequestTorque[1],(int)canRequestTorque[2],(int)canRequestTorque[3]);
-        
+
+        printf("%1.6f\r\n", getVelocity());
+
         time = timer.read();
         while(timer.read_ms() < CONTROL_CYCLE_MS);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止