Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN
Dependents: hairobo2021_F446re
Revision 0:b5561184f201, committed 2021-11-26
- 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