Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN

Dependents:   hairobo2021_F446re

Files at this revision

API Documentation at this revision

Comitter:
st17099ng
Date:
Fri Nov 26 00:29:28 2021 +0000
Child:
1:132886173bec
Commit message:
First commit.

Changed in this revision

TrackedVehicle.cpp Show annotated file Show diff for this revision Revisions of this file
TrackedVehicle.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TrackedVehicle.cpp	Fri Nov 26 00:29:28 2021 +0000
@@ -0,0 +1,253 @@
+#include "TrackedVehicle.h"
+
+TrackedVehicle::TrackedVehicle(uint16_t rightID, uint16_t leftID) : RightID(rightID), LeftID(leftID), RightMsg(rightID, RightData, 3, CANData, CANStandard), LeftMsg(leftID, LeftData, 3, CANData, CANStandard)
+{
+    ModuleStatus = INITIAL_STATUS;
+    CWDuty  = CW30_DUTY;
+    CCWDuty = CCW30_DUTY;
+    EncoderPulse = 4096;
+    MaxTargetPulse = EncoderPulse + 113;
+    MinTargetPulse = EncoderPulse - 113;
+    RevolutionPulse = 8192;
+
+    RightData[0] = 0x00;            //MD Operation Status
+    RightData[1] = rightID - 0x7DF; //MD Number
+    RightData[2] = BREAK_DUTY;      //MD Duty Data
+    RightData[3] = 0x00;            //Module Pulse High byte
+    RightData[4] = 0x00;            //Module Pulse Low byte
+    RightData[5] = 0x00;            //Empty Data
+    RightData[6] = 0x00;            //Empty Data
+    RightData[7] = 0x00;            //Empty Data
+
+    LeftData[0]  = 0x00;            //MD Operation Status
+    LeftData[1]  = leftID - 0x7DF;  //MD Number
+    LeftData[2]  = BREAK_DUTY;      //MD Duty Data
+    LeftData[3]  = 0x00;            //Module Pulse High byte
+    LeftData[4]  = 0x00;            //Module Pulse Lower byte
+    LeftData[5]  = 0x00;            //Empty Data
+    LeftData[6]  = 0x00;            //Empty Data
+    LeftData[7]  = 0x00;            //Empty Data
+
+
+
+}
+
+void TrackedVehicle::Stop_Robot(void)
+{
+    RightMsg.data[DUTY] = BREAK_DUTY;
+    LeftMsg.data[DUTY] = BREAK_DUTY;
+}
+
+
+
+void TrackedVehicle::Convert_PS3toMsg(short status, bool r, bool l, int8_t joystick)
+{
+    char JoystickDuty = joystick*2+0x7F;
+    if( (EncoderPulse > MinTargetPulse) && (EncoderPulse < MaxTargetPulse) || status == MANUAL_STATUS) {
+        /*  信地右旋回  */
+        if( ( r == 1 ) && ( JoystickDuty >= 0xBF ) ) {
+
+            RightMsg.data[DUTY] = BREAK_DUTY;
+            LeftMsg.data[DUTY] = CCWDuty;
+
+            /*  信地左旋回   */
+        } else if( ( l == 1 ) && ( JoystickDuty >= 0xBF ) ) {
+
+            RightMsg.data[DUTY] = CCWDuty;
+            LeftMsg.data[DUTY] = BREAK_DUTY;
+
+            /*  超信地右旋回  */
+        } else if( r == 1) {
+
+            RightMsg.data[DUTY] = CWDuty;
+            LeftMsg.data[DUTY] = CCWDuty;
+
+            /*  超信地左旋回  */
+        } else if( l == 1 ) {
+
+            RightMsg.data[DUTY] = CCWDuty;
+            LeftMsg.data[DUTY] = CWDuty;
+
+            /*  前進(JoyStickの閾値32の時  */
+            /*  計算式:DEC(32*2) + HEX(0x7F) = 0xBF  */
+        } else if( JoystickDuty >= 0xBF ) {
+
+            RightMsg.data[DUTY] = CCWDuty;
+            LeftMsg.data[DUTY] = CCWDuty;
+
+            /* 後進(JoyStickの閾値-32の時 */
+            /*  計算式:DEC(-32*2) + HEX(0x7F) = 0x3F  */
+        } else if( JoystickDuty <= 0x3F ) {
+
+            RightMsg.data[DUTY] = CWDuty;
+            LeftMsg.data[DUTY] = CWDuty;
+
+            /*  停止  */
+        } else {
+
+            RightMsg.data[DUTY] = BREAK_DUTY;
+            LeftMsg.data[DUTY] = BREAK_DUTY;
+
+        }
+    } else {
+        Control_Angle();
+    }
+}
+
+void TrackedVehicle::Control_Angle(void)
+{
+    if( EncoderPulse > MaxTargetPulse ) {
+        /*  超信地左旋回  */
+        RightMsg.data[DUTY] = CCWDuty;
+        LeftMsg.data[DUTY] = CWDuty;
+    } else if(EncoderPulse < MinTargetPulse ) {
+        /*  超信地右旋回  */
+        RightMsg.data[DUTY] = CWDuty;
+        LeftMsg.data[DUTY] = CCWDuty
+        ;
+
+    } else {
+        RightMsg.data[DUTY] = BREAK_DUTY;
+        LeftMsg.data[DUTY] = BREAK_DUTY;
+    }
+}
+
+void TrackedVehicle::Control_Turn(short status)
+{
+    ModuleStatus = status;
+    //指定角度内
+    if( (EncoderPulse > MinTargetPulse) && (EncoderPulse < MaxTargetPulse) ) {
+        if( ModuleStatus == RIGHT_SOFT_TURN ) {
+            //全体右緩旋回
+            RightMsg.data[DUTY] = CCW20_DUTY;
+            LeftMsg.data[DUTY]  = CCW40_DUTY;
+        } else if( ModuleStatus == RIGHT_SOFT_TURN_BACK ) {
+            //全体後ろ右緩旋回
+            RightMsg.data[DUTY] = CW20_DUTY;
+            LeftMsg.data[DUTY]  = CW40_DUTY;
+        }  else if( ModuleStatus == LEFT_SOFT_TURN ) {
+            //全体左緩旋回
+            RightMsg.data[DUTY] = CCW40_DUTY;
+            LeftMsg.data[DUTY]  = CCW20_DUTY;
+        }  else if( ModuleStatus == LEFT_SOFT_TURN_BACK ) {
+            //全体後ろ左緩旋回
+            RightMsg.data[DUTY] = CW40_DUTY;
+            LeftMsg.data[DUTY]  = CW20_DUTY;
+        } else {
+            RightMsg.data[DUTY] = BREAK_DUTY;
+            LeftMsg.data[DUTY] = BREAK_DUTY;
+        }
+        /* ここから */
+    } else if( EncoderPulse < MinTargetPulse && (ModuleStatus == RIGHT_SOFT_TURN || ModuleStatus == LEFT_SOFT_TURN) ) {
+        RightMsg.data[DUTY] = CCW10_DUTY;
+        LeftMsg.data[DUTY] = CCW30_DUTY;
+    } else if( EncoderPulse < MinTargetPulse && (ModuleStatus == RIGHT_SOFT_TURN_BACK || ModuleStatus == LEFT_SOFT_TURN_BACK) ) {
+        RightMsg.data[DUTY] = CW30_DUTY;
+        LeftMsg.data[DUTY] = CW10_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (ModuleStatus == RIGHT_SOFT_TURN || ModuleStatus == LEFT_SOFT_TURN) ) {
+        RightMsg.data[DUTY] = CCW30_DUTY;
+        LeftMsg.data[DUTY] = CCW10_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (ModuleStatus == RIGHT_SOFT_TURN_BACK || ModuleStatus == LEFT_SOFT_TURN_BACK) ) {
+        RightMsg.data[DUTY] = CW10_DUTY;
+        LeftMsg.data[DUTY] = CW30_DUTY;
+        /* ここまでは緩旋回での角度修正 */
+    } else {
+        //緩旋回以外の時の角度修正
+        Control_Angle();
+    }
+}
+
+void TrackedVehicle::Control_Robot_Turn(short status, short robot_status)
+{
+    ModuleStatus = status;
+    if( (EncoderPulse > MinTargetPulse) && (EncoderPulse < MaxTargetPulse) ) {
+        if( robot_status == ROBOT_CW_TURN && status == MODULE_FRONT ) {
+            //全体CWのModule1
+            RightMsg.data[DUTY] = CCW20_DUTY;
+            LeftMsg.data[DUTY]  = CCW40_DUTY;
+        } else if( robot_status == ROBOT_CW_TURN && status == MODULE_BACK ) {
+            //全体CWのModule2
+            RightMsg.data[DUTY] = CW40_DUTY;
+            LeftMsg.data[DUTY]  = CW20_DUTY;
+        } else if( robot_status == ROBOT_CCW_TURN && status == MODULE_FRONT ) {
+            //全体CCWのModule2
+            RightMsg.data[DUTY] = CCW40_DUTY;
+            LeftMsg.data[DUTY]  = CCW20_DUTY;
+        } else if( robot_status == ROBOT_CCW_TURN && status == MODULE_BACK ) {
+            //全体CCWのModule1
+            RightMsg.data[DUTY] = CW20_DUTY;
+            LeftMsg.data[DUTY]  = CW40_DUTY;
+        }
+        /* ここから */
+    } else if( EncoderPulse < MinTargetPulse && (robot_status == ROBOT_CW_TURN && status == MODULE_FRONT) ) {
+        RightMsg.data[DUTY] = CCW20_DUTY;
+        LeftMsg.data[DUTY]  = CCW50_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (robot_status == ROBOT_CW_TURN && status == MODULE_FRONT) ) {
+        RightMsg.data[DUTY] = CCW50_DUTY;
+        LeftMsg.data[DUTY]  = CCW20_DUTY;
+    } else if( EncoderPulse < MinTargetPulse && (robot_status == ROBOT_CW_TURN && status == MODULE_BACK) ) {
+        RightMsg.data[DUTY] = CW50_DUTY;
+        LeftMsg.data[DUTY]  = CW20_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (robot_status == ROBOT_CW_TURN && status == MODULE_BACK) ) {
+        RightMsg.data[DUTY] = CW20_DUTY;
+        LeftMsg.data[DUTY]  = CW50_DUTY;
+    } else if( EncoderPulse < MinTargetPulse && (robot_status == ROBOT_CCW_TURN && status == MODULE_FRONT) ) {
+        RightMsg.data[DUTY] = CCW20_DUTY;
+        LeftMsg.data[DUTY]  = CCW50_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (robot_status == ROBOT_CCW_TURN && status == MODULE_FRONT) ) {
+        RightMsg.data[DUTY] = CCW50_DUTY;
+        LeftMsg.data[DUTY]  = CCW20_DUTY;
+    } else if( EncoderPulse < MinTargetPulse && (robot_status == ROBOT_CCW_TURN && status == MODULE_BACK) ) {
+        RightMsg.data[DUTY] = CW50_DUTY;
+        LeftMsg.data[DUTY]  = CW20_DUTY;
+    } else if( EncoderPulse > MaxTargetPulse && (robot_status == ROBOT_CCW_TURN && status == MODULE_BACK) ) {
+        RightMsg.data[DUTY] = CW20_DUTY;
+        LeftMsg.data[DUTY]  = CW50_DUTY;
+    }
+    /* ここまで各動作の角度修正 */
+}
+
+
+void TrackedVehicle::Set_EncoderPulse(uint16_t pulse)
+{
+    EncoderPulse = pulse;
+}
+
+void TrackedVehicle::Set_TargetPulse(uint16_t pulse)
+{
+    TargetPulse = pulse;
+    MaxTargetPulse = pulse + 113;
+    MinTargetPulse = pulse - 113;
+}
+
+void TrackedVehicle::Set_TargetAngle(float angle)
+{
+    if( angle > -90.0f && angle < 90.0f ) {
+        Set_TargetPulse( 4096+(angle/360) * RevolutionPulse );
+    }
+}
+
+uint16_t TrackedVehicle::Get_ModulePulse(void)
+{
+    return EncoderPulse;
+}
+
+float TrackedVehicle::Get_ModuleAngle(void)
+{
+    return -180.0f - (float)360.0f/RevolutionPulse * (float)EncoderPulse;
+}
+
+short TrackedVehicle::Get_ControlStatus(void)
+{
+    return ModuleStatus;
+}
+
+CANMessage* TrackedVehicle::Get_RightMsg(void)
+{
+    return &RightMsg;
+}
+
+CANMessage* TrackedVehicle::Get_LeftMsg(void)
+{
+    return &LeftMsg;
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TrackedVehicle.h	Fri Nov 26 00:29:28 2021 +0000
@@ -0,0 +1,92 @@
+#ifndef TRACKED_VEHICLE_H
+#define TRACKED_VEHICLE_H
+
+#include "mbed.h"
+
+class TrackedVehicle
+{
+private:
+    enum MsgData {
+        STATUS = 0,
+        NUM,
+        DUTY,
+        PULSE_HIGH_BYTE,
+        PULSE_LOW_BYTE,
+        EMPTY1,
+        EMPTY2,
+        EMPTY3
+    };
+    enum sequence_number {
+        STOP_STATUS,
+        INITIAL_STATUS,
+        MANUAL_STATUS,
+        ZERO_POINT_TURN,
+        OPPOSITE_PHASE_RIGHT_TURN,
+        RIGHT_SOFT_TURN,
+        RIGHT_SOFT_TURN_BACK,
+        OPPOSITE_PHASE_LEFT_TURN,
+        LEFT_SOFT_TURN,
+        LEFT_SOFT_TURN_BACK,
+        RIGHT90_TURN,
+        LEFT90_TURN,
+        MODULE_FRONT,
+        MODULE_BACK,
+        ROBOT_CW_TURN,
+        ROBOT_CCW_TURN
+    };
+    short ModuleStatus;
+    char CWDuty;
+    char CCWDuty;
+    char RightData[8];
+    char LeftData[8];
+    uint16_t RevolutionPulse;
+    uint16_t EncoderPulse;
+    uint16_t TargetPulse;
+    uint16_t MinTargetPulse;
+    uint16_t MaxTargetPulse;
+    const uint16_t RightID;
+    const uint16_t LeftID;
+    float TargetAngle;
+    CANMessage RightMsg;
+    CANMessage LeftMsg;
+
+public:
+    enum MD_DUTY {
+        CW95_DUTY       = 0xF8,
+        CW80_DUTY       = 0xE4,
+        CW70_DUTY       = 0xD7,
+        CW60_DUTY       = 0xCA,
+        CW50_DUTY       = 0xBE,
+        CW40_DUTY       = 0xB3,
+        CW30_DUTY       = 0xA6,
+        CW20_DUTY       = 0x99,
+        CW10_DUTY       = 0x8C,
+        BREAK_DUTY      = 0x7F,
+        CCW10_DUTY      = 0x72,
+        CCW20_DUTY      = 0x65,
+        CCW30_DUTY      = 0x58,
+        CCW40_DUTY      = 0x4B,
+        CCW50_DUTY      = 0x3F,
+        CCW60_DUTY      = 0x32,
+        CCW70_DUTY      = 0x26,
+        CCW80_DUTY      = 0x19,
+        CCW95_DUTY      = 0x07,
+        ADJUST_DUTY     = 0x03
+    };
+    TrackedVehicle(uint16_t rightID, uint16_t leftID);
+    void Stop_Robot(void);
+    void Convert_PS3toMsg(short status, bool r, bool l, int8_t joystick);
+    void Control_Angle(void);
+    void Control_Turn(short status);
+    void Control_Robot_Turn(short status, short robot_status);
+    void Set_EncoderPulse(uint16_t pulse);
+    void Set_TargetPulse(uint16_t pulse);
+    void Set_TargetAngle(float angle);
+    uint16_t Get_ModulePulse(void);
+    float Get_ModuleAngle(void);
+    short Get_ControlStatus(void);
+    CANMessage* Get_RightMsg(void);
+    CANMessage* Get_LeftMsg(void);
+};
+
+#endif
\ No newline at end of file