Hairo ROBOCON2021 TrackedVehicle moudule library. Ibaraki KOSEN
Dependencies: mbed TrackedVehicle PS3
Revision 0:fe9b54865125, committed 2021-11-26
- Comitter:
- st17099ng
- Date:
- Fri Nov 26 00:35:42 2021 +0000
- Child:
- 1:ca42cf6ae77e
- Commit message:
- First commit hairobo2021_NUCLEO-F446RE
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PS3.lib Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/st17099ng/code/PS3/#0fdf068ce9f4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/README.h Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,197 @@ +/************************************************************************************************** + +program変更履歴 +-------------------------------------------------------------------- + 2021/10/11 R.I.P. + TrackedVehicleにConvert_PS3toMsgとして移行 + PS3Data -> Duty変換関数 +void Data_To_Duty(void) +{ + bool r1 = ps3.getButtonState( R1 ); + bool l1 = ps3.getButtonState( L1 ); + bool r2 = ps3.getButtonState( R2 ); + bool l2 = ps3.getButtonState( L2 ); + MD1_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F; + MD2_MaxDuty = ps3.getRightJoystickYaxis()*2+0x7F; + MD3_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F; + MD4_MaxDuty = ps3.getLeftJoystickYaxis()*2+0x7F; + + モジュール1 + 信地右旋回 + if( ( r1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) { + + MD1_Duty = BREAK_DUTY; + MD2_Duty = HALF_CCW_DUTY; + + 信地左旋回 + } else if( ( l1 == 1 ) && ( MD1_MaxDuty >= 0xBF ) ) { + + MD1_Duty = HALF_CCW_DUTY; + MD2_Duty = BREAK_DUTY; + + 超信地右旋回 + } else if( r1 == 1) { + + MD1_Duty = HALF_CW_DUTY; + MD2_Duty = HALF_CCW_DUTY; + + 超信地左旋回 + } else if( l1 == 1 ) { + + MD1_Duty = HALF_CCW_DUTY; + MD2_Duty = HALF_CW_DUTY; + + 前進(JoyStickの閾値32の時 + 計算式:DEC(32*2) + HEX(0x7F) = 0xBF + } else if( MD1_MaxDuty >= 0xBF ) { + +// MD1_Duty = CCW10_DUTY; + MD1_Duty = HALF_CCW_DUTY; + MD2_Duty = HALF_CCW_DUTY; + + 後進(JoyStickの閾値-32の時 + 計算式:DEC(-32*2) + HEX(0x7F) = 0x3F + } else if( MD1_MaxDuty <= 0x3F ) { + + MD1_Duty = CW10_DUTY; + MD1_Duty = HALF_CW_DUTY; + MD2_Duty = HALF_CW_DUTY; + + 停止 + } else { + + MD1_Duty = BREAK_DUTY; + MD2_Duty = BREAK_DUTY; + + } + + + モジュール2 + 信地右旋回 + if( ( r2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) { + + MD3_Duty = BREAK_DUTY; + MD4_Duty = HALF_CCW_DUTY; + + 信地左旋回 + } else if( ( l2 == 1 ) && ( MD3_MaxDuty >= 0xBF ) ) { + + MD3_Duty = HALF_CCW_DUTY; + MD4_Duty = BREAK_DUTY; + + 超信地右旋回 + } else if( r2 == 1) { + + MD3_Duty = HALF_CW_DUTY; + MD4_Duty = HALF_CCW_DUTY; + + 超信地左旋回 + } else if( l2 == 1 ) { + + MD3_Duty = HALF_CCW_DUTY; + MD4_Duty = HALF_CW_DUTY; + + 前進(JoyStickの閾値32の時 + 計算式:DEC(32*2) + HEX(0x7F) = 0xBF + } else if( MD3_MaxDuty >= 0xBF ) { + + MD3_Duty = HALF_CCW_DUTY; + MD4_Duty = HALF_CCW_DUTY; + + 後進(JoyStickの閾値-32の時 + 計算式:DEC(-32*2) + HEX(0x7F) = 0x3F + } else if( MD3_MaxDuty <= 0x3F ) { + + MD3_Duty = HALF_CW_DUTY; + MD4_Duty = HALF_CW_DUTY; + + 停止 + } else { + + MD3_Duty = BREAK_DUTY; + MD4_Duty = BREAK_DUTY; + + } +} +---台形制御をMDのプログラムに導入したため--- +---台形制御関数--- +void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty) +{ + int Diff_Duty = MD_Duty - msg->data[2]; + if ( msg->data[2] == MD_Duty ) { + + msg->data[2] = MD_Duty; + + } else if( Diff_Duty >= 3 ) { + + msg->data[2] += ADJUST_DUTY; + + } else if( ( Diff_Duty < 3 ) && ( Diff_Duty > 0 ) ) { + + msg->data[2] += Diff_Duty; + + } else if( ( Diff_Duty > -3 ) && ( Diff_Duty < 0 ) ) { + + msg->data[2] += Diff_Duty; + + } else if( Diff_Duty <= 3 ) { + + msg->data[2] -= ADJUST_DUTY; + + } +} + 2021/08/26 R.I.P. +--void Soft_Duty(void) -> void Trapezoid_Ctrl(CANMessage *msg, char MD_Duty) に変更--- +void Soft_Duty(void) +{ + if(msg1.data[2] == MD1_Duty) { + msg1.data[2] = MD1_Duty; + } else if(msg1.data[2] <= MD1_Duty) { + msg1.data[2] += ADJUST_DUTY; + } else if(msg1.data[2] >= MD1_Duty) { + msg1.data[2] -= ADJUST_DUTY; + } + + if(msg2.data[2] == MD2_Duty) { + msg2.data[2] = MD2_Duty; + } else if(msg2.data[2] <= MD2_Duty) { + msg2.data[2] += ADJUST_DUTY; + } else if(msg2.data[2] >= MD2_Duty) { + msg2.data[2] -= ADJUST_DUTY; + } +} + + +---[実装未定]PS3コン切替式非常停止--- + if( Old_Emg_Flag != Emg_Flag ) { + if( ( Old_Emg_Flag == 0 ) && ( Emg_Flag == 1 ) ) {//立ち上がり + led1 = 0; + emg_stop_tri = 0;//停止 + } else if( ( Old_Emg_Flag == 1 ) && ( Emg_Flag == 0 ) ) {//立下り + led1 = 0; + emg_stop_tri = 0;//停止 + } + } else { + Old_Emg_Flag = Emg_Flag; + } + + +---ノードが増えると割込みによる運用が出来なくなる--- + ticker.attach(&CAN1_Trans, 0.05); +---CAN1送信関数--- +void CAN1_Trans(void) +{ + if( !(can1.write(msg1)) ) { + pc.printf("\033[1;1Hmsg1 failed"); + } + if( !(can1.write(msg2)) ) { + pc.printf("\033[2;1Hmsg2 failed"); + } + if( !(can1.write(msg3)) ) { + pc.printf("\033[3;1Hmsg3 failed"); + } + if( !(can1.write(msg4)) ) { + pc.printf("\033[4;1Hmsg4 failed"); + } +} +***************************************************************************************************/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robot_Definition.h Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,50 @@ +#ifndef ROBOT_DEFINITION_H +#define ROBOT_DEFINITION_H + +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 +}; + +enum ControlNumber { + PS3MODE, + GUIMODE +}; + +enum 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 +}; +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TrackedVehicle.lib Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/st17099ng/code/TrackedVehicle/#b5561184f201
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,617 @@ +#include "mbed.h" +#include "TrackedVehicle.h" +#include "Robot_Definition.h" +#include "PS3.h" + +void Update_Robot_Status(void); +void Lift_Control(void); +void Paint_Control(void); +void Cable_Control(void); +void Outrigger_Control(void); +void GUI_to_Status(void); +void CAN1_Trans(CANMessage *msg); +void CAN1_Rcv(void); +void GUI_Receive(void); +void GUI_Safety_Stop(void); +void Buzzer_Toggle(void); +void Emg_Stop(void); + + +char GUIData = 0x00; +char ResetData[8] = {0, 0, BREAK_DUTY, 0,0,0,0,0}; +char MD5Data[8] = {0, 5, BREAK_DUTY, 0,0,0,0,0}; +char MD6Data[8] = {0, 6, BREAK_DUTY, 0,0,0,0,0}; +char MD7Data[8] = {0, 7, BREAK_DUTY, 0,0,0,0,0}; +char MD8Data[8] = {0, 8, BREAK_DUTY, 0,0,0,0,0}; +char MD9Data[8] = {0, 9, BREAK_DUTY, 0,0,0,0,0}; +char MD10Data[8] = {0, 10, BREAK_DUTY, 0,0,0,0,0}; +int8_t RightJoyX = 0; +int8_t RightJoyY = 0; +int8_t LeftJoyX = 0; +int8_t LeftJoyY = 0; +uint16_t Module1Encoder = 4096; +uint16_t Module2Encoder = 4096; +uint16_t LiftEncoder = 2400; +short ControlMode = GUIMODE; +short RobotStatus = INITIAL_STATUS; +short Old_RobotStatus = INITIAL_STATUS; +short Module1Status = INITIAL_STATUS; +short Module2Status = INITIAL_STATUS; + +bool MaintainAngle = false;//sita +bool ZeroPointTurn = false;//ue +bool RightTurn = false;//migi +bool LeftTurn = false;//hidari +bool CWTurn = false;//R1 +bool CCWTurn = false;//L1 +bool OppositePhase_RightTurn = false;//maru +bool OppositePhase_LeftTurn = false;//sikaku +bool Right2 = false;//R2 +bool Left2 = false;//L2 +bool LiftUp = false;//sankaku +bool LiftDown = false;//batsu +bool Old_MaintainAngle = false; +bool Old_ZeroPointTurn = false; +bool Old_RightTurn = false; +bool Old_LeftTurn = false; +bool Old_CWTurn = false; +bool Old_CCWTurn = false; +bool Old_OppositePhase_RightTurn = false; +bool Old_OppositePhase_LeftTurn = false; +bool Old_LiftUp = false; +bool Old_LiftDown = false; + +bool EmgFlag = true; +bool Start = false; +bool Select = false; +bool Old_Start = false; +bool Old_Select = false; +bool Old_emg_stop_sw = false; + +Ticker GUI_Safety_Timer; +Ticker EMG_Buzzer_Timer; +CAN can1(PA_11, PA_12,500000); +Serial pc(USBTX,USBRX,115200); +Serial RaspPi(PC_12, PD_2,115200); + +PS3 ps3(PC_10,PC_11); +TrackedVehicle Module1(0x7E0, 0x7E1); +TrackedVehicle Module2(0x7E2, 0x7E3); + +DigitalOut led1(LED1); +DigitalOut emg_stop_tri(PB_14); +DigitalOut Ps3_Led(PC_0); +DigitalOut Pwr_Led(PC_1); +DigitalOut Emg_Led(PB_0); +DigitalOut Gui_Led(PA_4); +DigitalOut buzzer(PA_1); +DigitalIn emg_stop_sw(PB_13); + +CANMessage Rcv_Msg; +CANMessage MD1_Msg(0x7E0,ResetData,3,CANData,CANStandard);//Module1Right +CANMessage MD2_Msg(0x7E1,ResetData,3,CANData,CANStandard);//Module1Left +CANMessage MD3_Msg(0x7E2,ResetData,3,CANData,CANStandard);//Module2Right +CANMessage MD4_Msg(0x7E3,ResetData,3,CANData,CANStandard);//Module2Left +CANMessage MD5_Msg(0x7E4,MD5Data,3,CANData,CANStandard);//lift +CANMessage MD6_Msg(0x7E5,MD6Data,3,CANData,CANStandard);//X +CANMessage MD7_Msg(0x7E6,MD7Data,3,CANData,CANStandard);//Y +CANMessage MD8_Msg(0x7E7,MD8Data,3,CANData,CANStandard);//Z +CANMessage MD9_Msg(0x7E8,MD9Data,3,CANData,CANStandard);//Cable +CANMessage MD10_Msg(0x7E9,MD10Data,3,CANData,CANStandard);//Outrigger + +int main() +{ + emg_stop_tri = 0; + Ps3_Led = 0; + Pwr_Led = 1; + Emg_Led = 1; + Gui_Led = 1; + emg_stop_sw.mode( PullUp ); +// コメント外すとうるさいよ +// EMG_Buzzer_Timer.attach(&Buzzer_Toggle, 1.5); + can1.attach(&CAN1_Rcv,CAN::RxIrq); +// pc.attach(&GUI_Receive,Serial::RxIrq); + RaspPi.attach(&GUI_Receive,Serial::RxIrq); + CAN1_Trans(&MD1_Msg); + CAN1_Trans(&MD2_Msg); + CAN1_Trans(&MD3_Msg); + CAN1_Trans(&MD4_Msg); + CAN1_Trans(&MD5_Msg); + CAN1_Trans(&MD6_Msg); + CAN1_Trans(&MD7_Msg); + CAN1_Trans(&MD8_Msg); + CAN1_Trans(&MD9_Msg); + CAN1_Trans(&MD10_Msg); + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET); + while(1) { + Emg_Stop(); + if(ControlMode == PS3MODE) { + Update_Robot_Status(); + } else if(ControlMode == GUIMODE) { + GUI_to_Status(); + } + Lift_Control(); + Paint_Control(); + Cable_Control(); + Outrigger_Control(); + switch (RobotStatus) { + case STOP_STATUS: + Module1.Stop_Robot(); + Module2.Stop_Robot(); + break; + //1,2(前後のみ) + case ZERO_POINT_TURN: + Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY); + Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY); + break; + + //3,4,5,6(左右緩旋回前後) + case OPPOSITE_PHASE_RIGHT_TURN: + case OPPOSITE_PHASE_LEFT_TURN: + case RIGHT_SOFT_TURN: + case LEFT_SOFT_TURN: + case RIGHT_SOFT_TURN_BACK: + case LEFT_SOFT_TURN_BACK: + Module1.Control_Turn(Module1Status); + Module2.Control_Turn(Module2Status); + break; + + //7,8(90°旋回と前後) + case RIGHT90_TURN: + case LEFT90_TURN: + Module1.Convert_PS3toMsg(Module1Status, false, false, LeftJoyY); + Module2.Convert_PS3toMsg(Module2Status, false, false, LeftJoyY); + break; + + //9,10(全体CW,CW) + case ROBOT_CW_TURN: + case ROBOT_CCW_TURN: + Module1.Control_Robot_Turn(Module1Status, RobotStatus); + Module2.Control_Robot_Turn(Module2Status, RobotStatus); + break; + + //ワカラン + case INITIAL_STATUS: + Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY); + Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY); + break; + + //マニュアル操作 + case MANUAL_STATUS: + Module1.Convert_PS3toMsg(Module1Status, CWTurn, CCWTurn, RightJoyY); + Module2.Convert_PS3toMsg(Module2Status, Right2, Left2, LeftJoyY); + break; + } + CAN1_Trans(Module1.Get_RightMsg()); + CAN1_Trans(Module1.Get_LeftMsg()); + CAN1_Trans(Module2.Get_RightMsg()); + CAN1_Trans(Module2.Get_LeftMsg()); + CAN1_Trans(&MD5_Msg); + CAN1_Trans(&MD6_Msg); + CAN1_Trans(&MD7_Msg); + CAN1_Trans(&MD8_Msg); + CAN1_Trans(&MD9_Msg); + CAN1_Trans(&MD10_Msg); + } +} +/* ここまでmain文 */ + +/* PS3 -> ロボットステータス更新関数 */ +void Update_Robot_Status(void) +{ + MaintainAngle = ps3.getButtonState( sita ); + ZeroPointTurn = ps3.getButtonState( ue ); + RightTurn = ps3.getButtonState( migi ); + LeftTurn = ps3.getButtonState( hidari ); + CWTurn = ps3.getButtonState( R1 ); + CCWTurn = ps3.getButtonState( L1 ); + Right2 = ps3.getButtonState( R2 ); + Left2 = ps3.getButtonState( L2 ); + OppositePhase_RightTurn = ps3.getButtonState( maru ); + OppositePhase_LeftTurn = ps3.getButtonState( sikaku ); + RightJoyY = ps3.getRightJoystickYaxis(); + LeftJoyY = ps3.getLeftJoystickYaxis(); + + if( Start != true && Select != true) { + if( (ZeroPointTurn == true) && (Old_ZeroPointTurn == false) ) { + //正面 + Module1.Set_TargetPulse(4096); + Module2.Set_TargetPulse(4096); + Module1Status = ZERO_POINT_TURN; + Module2Status = ZERO_POINT_TURN; + RobotStatus = ZERO_POINT_TURN; + } else if( (RightTurn == true) && (Old_RightTurn == false) ) { + //右旋回90° + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = RIGHT90_TURN; + Module2Status = RIGHT90_TURN; + RobotStatus = RIGHT90_TURN; +// } else if( (LeftTurn == true) && (Old_LeftTurn == false) ) { +// //左旋回90° +// Module1.Set_TargetPulse(2048); +// Module2.Set_TargetPulse(2048); +// Module1Status = LEFT90_TURN; +// Module2Status = LEFT90_TURN; +// RobotStatus = LEFT90_TURN; + } else if( (OppositePhase_RightTurn == true) && (Old_OppositePhase_RightTurn == false) ) { + //逆位相右旋回±30° + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = OPPOSITE_PHASE_RIGHT_TURN; + Module2Status = OPPOSITE_PHASE_RIGHT_TURN; + RobotStatus = OPPOSITE_PHASE_RIGHT_TURN; + } else if( (OppositePhase_LeftTurn == true) && (Old_OppositePhase_LeftTurn == false) ) { + //逆位相左旋回±30° + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = OPPOSITE_PHASE_LEFT_TURN; + Module2Status = OPPOSITE_PHASE_LEFT_TURN; + RobotStatus = OPPOSITE_PHASE_LEFT_TURN; + } else if( LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) { + //全体右緩旋回 + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = RIGHT_SOFT_TURN; + Module2Status = RIGHT_SOFT_TURN; + RobotStatus = RIGHT_SOFT_TURN; + } else if( LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_RIGHT_TURN ) { + //全体後ろ右緩旋回 + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = RIGHT_SOFT_TURN_BACK; + Module2Status = RIGHT_SOFT_TURN_BACK; + RobotStatus = RIGHT_SOFT_TURN_BACK; + } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == RIGHT_SOFT_TURN || RobotStatus == RIGHT_SOFT_TURN_BACK) ) { + //全体右緩旋回停止->逆位相右旋回±30° + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = OPPOSITE_PHASE_RIGHT_TURN; + Module2Status = OPPOSITE_PHASE_RIGHT_TURN; + RobotStatus = OPPOSITE_PHASE_RIGHT_TURN; + } else if(LeftJoyY > 32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) { + //全体左緩旋回 + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = LEFT_SOFT_TURN; + Module2Status = LEFT_SOFT_TURN; + RobotStatus = LEFT_SOFT_TURN; + } else if(LeftJoyY < -32 && RobotStatus == OPPOSITE_PHASE_LEFT_TURN ) { + //全体後ろ左緩旋回 + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = LEFT_SOFT_TURN_BACK; + Module2Status = LEFT_SOFT_TURN_BACK; + RobotStatus = LEFT_SOFT_TURN_BACK; + } else if( (LeftJoyY > -32 && LeftJoyY < 32) && (RobotStatus == LEFT_SOFT_TURN || RobotStatus == LEFT_SOFT_TURN_BACK) ) { + //全体左緩旋回停止->逆位相左旋回±45° + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = OPPOSITE_PHASE_LEFT_TURN; + Module2Status = OPPOSITE_PHASE_LEFT_TURN; + RobotStatus = OPPOSITE_PHASE_LEFT_TURN; + } else if( (CWTurn == true) && (RobotStatus == RIGHT90_TURN) ) { + //全体CW + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = MODULE_FRONT; + Module2Status = MODULE_BACK; + RobotStatus = ROBOT_CW_TURN; + } else if( (CCWTurn == true) && (RobotStatus == RIGHT90_TURN) ) { + //全体CCW + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = MODULE_BACK; + Module2Status = MODULE_FRONT; + RobotStatus = ROBOT_CCW_TURN; + } else if( (CWTurn != true && CCWTurn != true) && (RobotStatus == ROBOT_CW_TURN || RobotStatus == ROBOT_CCW_TURN) ) { + //全体左右超信地旋回 -> 右旋回90° + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = RIGHT90_TURN; + Module2Status = RIGHT90_TURN; + RobotStatus = RIGHT90_TURN; + } else if( (MaintainAngle == true) && (Old_MaintainAngle == false) ) { + //十字ボタン下でその角度維持 + Module1.Set_TargetPulse(Module1.Get_ModulePulse()); + Module2.Set_TargetPulse(Module2.Get_ModulePulse()); + Module1Status = INITIAL_STATUS; + Module2Status = INITIAL_STATUS; + RobotStatus = INITIAL_STATUS; + } + } else if( (Start == true) && (Old_Start == false) ) { + Module1Status = MANUAL_STATUS; + Module2Status = MANUAL_STATUS; + RobotStatus = MANUAL_STATUS; + } + Old_MaintainAngle = MaintainAngle; + Old_ZeroPointTurn = ZeroPointTurn; + Old_RightTurn = RightTurn; + Old_LeftTurn = LeftTurn; + Old_CWTurn = CWTurn; + Old_CCWTurn = CCWTurn; + Old_OppositePhase_RightTurn = OppositePhase_RightTurn; + Old_OppositePhase_LeftTurn = OppositePhase_LeftTurn; + Old_Start = Start; +} + +/* Liftの処理関数 */ +void Lift_Control(void) +{ + LiftUp = ps3.getButtonState( sankaku ); + LiftDown = ps3.getButtonState( batu ); + if( LiftUp == true || GUIData == 0x0C ) { + //LIFT UP + MD5_Msg.data[2] = CW50_DUTY; + } else if( LiftDown == true || GUIData == 0x0D ) { + //LIFT DOWN + MD5_Msg.data[2] = CCW50_DUTY; + } else { + MD5_Msg.data[2] = BREAK_DUTY; + } +} + +/* Paintの処理関数 */ +void Paint_Control(void) +{ + MD6_Msg.data[2] = BREAK_DUTY; + MD7_Msg.data[2] = BREAK_DUTY; + MD8_Msg.data[2] = BREAK_DUTY; + if(GUIData == 0x0F) { + MD6_Msg.data[2] = CW50_DUTY; + } else if(GUIData == 0x10) { + MD6_Msg.data[2] = CCW50_DUTY; + } else if(GUIData == 0x11) { + MD7_Msg.data[2] = CW50_DUTY; + } else if(GUIData == 0x12) { + MD7_Msg.data[2] = CCW50_DUTY; + } else if(GUIData == 0x13) { + MD8_Msg.data[2] = CW50_DUTY; + } else if(GUIData == 0x14) { + MD8_Msg.data[2] = CCW50_DUTY; + } else { + MD6_Msg.data[2] = BREAK_DUTY; + MD7_Msg.data[2] = BREAK_DUTY; + MD8_Msg.data[2] = BREAK_DUTY; + } +} +/* Cableの処理関数 */ +void Cable_Control(void) +{ + if(GUIData == 0x16) { + //Cable収納 + MD9_Msg.data[2] = CW50_DUTY; + } else if(GUIData == 0x17) { + //Cable排出 + MD9_Msg.data[2] = CCW50_DUTY; + } else { + MD9_Msg.data[2] = BREAK_DUTY; + } +} + +/* Outriggerの処理関数 */ +void Outrigger_Control(void) +{ + if(GUIData == 0x19) { + //Outrigger展開 + MD10_Msg.data[2] = CW50_DUTY; + } else if(GUIData == 0x1A) { + //Outrigger収納 + MD10_Msg.data[2] = CCW50_DUTY; + } else { + MD10_Msg.data[2] = BREAK_DUTY; + } +} + +/* GUI -> Status変換関数 */ +void GUI_to_Status(void) +{ + LeftJoyY = 0; + if( GUIData == 0x01 ) { + //前進 + Module1.Set_TargetPulse(4096); + Module2.Set_TargetPulse(4096); + Module1Status = ZERO_POINT_TURN; + Module2Status = ZERO_POINT_TURN; + RobotStatus = ZERO_POINT_TURN; + LeftJoyY = 63; + } else if( GUIData == 0x02 ) { + //後進 + Module1.Set_TargetPulse(4096); + Module2.Set_TargetPulse(4096); + Module1Status = ZERO_POINT_TURN; + Module2Status = ZERO_POINT_TURN; + RobotStatus = ZERO_POINT_TURN; + LeftJoyY = -63; + } else if( GUIData == 0x03 ) { + //右緩旋回 + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = RIGHT_SOFT_TURN; + Module2Status = RIGHT_SOFT_TURN; + RobotStatus = RIGHT_SOFT_TURN; + } else if( GUIData == 0x04 ) { + //右緩後旋回 + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = RIGHT_SOFT_TURN_BACK; + Module2Status = RIGHT_SOFT_TURN_BACK; + RobotStatus = RIGHT_SOFT_TURN_BACK; + } else if( GUIData == 0x05 ) { + //左緩旋回 + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = LEFT_SOFT_TURN; + Module2Status = LEFT_SOFT_TURN; + RobotStatus = LEFT_SOFT_TURN; + } else if( GUIData == 0x06 ) { + //左緩後旋回 + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = LEFT_SOFT_TURN_BACK; + Module2Status = LEFT_SOFT_TURN_BACK; + RobotStatus = LEFT_SOFT_TURN_BACK; + } else if( GUIData == 0x07 ) { + //右向き前進 + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = RIGHT90_TURN; + Module2Status = RIGHT90_TURN; + RobotStatus = RIGHT90_TURN; + LeftJoyY = 63; + } else if( GUIData == 0x08 ) { + //右向き後進 + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = RIGHT90_TURN; + Module2Status = RIGHT90_TURN; + RobotStatus = RIGHT90_TURN; + LeftJoyY = -63; + } else if( GUIData == 0x09 ) { + //全体CW + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = MODULE_FRONT; + Module2Status = MODULE_BACK; + RobotStatus = ROBOT_CW_TURN; + } else if( GUIData == 0x0A ) { + //全体CCW + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = MODULE_BACK; + Module2Status = MODULE_FRONT; + RobotStatus = ROBOT_CCW_TURN; + } else if( GUIData == 0xFF ) { + //0°(正面) + Module1.Set_TargetPulse(4096); + Module2.Set_TargetPulse(4096); + Module1Status = ZERO_POINT_TURN; + Module2Status = ZERO_POINT_TURN; + RobotStatus = ZERO_POINT_TURN; + } else if( GUIData == 0xFE ) { + //±30°(右緩旋回) + Module1.Set_TargetPulse(4778); + Module2.Set_TargetPulse(3414); + Module1Status = OPPOSITE_PHASE_RIGHT_TURN; + Module2Status = OPPOSITE_PHASE_RIGHT_TURN; + RobotStatus = OPPOSITE_PHASE_RIGHT_TURN; + } else if( GUIData == 0xFD ) { + //±30°(左緩旋回) + Module1.Set_TargetPulse(3414); + Module2.Set_TargetPulse(4778); + Module1Status = OPPOSITE_PHASE_LEFT_TURN; + Module2Status = OPPOSITE_PHASE_LEFT_TURN; + RobotStatus = OPPOSITE_PHASE_LEFT_TURN; + } else if( GUIData == 0xFC ) { + //90°(右向き) + Module1.Set_TargetPulse(6144); + Module2.Set_TargetPulse(6144); + Module1Status = RIGHT90_TURN; + Module2Status = RIGHT90_TURN; + RobotStatus = RIGHT90_TURN; + } else { + //停止 + Module1Status = STOP_STATUS; + Module2Status = STOP_STATUS; + RobotStatus = STOP_STATUS; + } +} + + + +/* CAN1送信関数 */ +void CAN1_Trans(CANMessage *msg) +{ + can1.write(*msg); + wait(0.0020); +// 255bit/500kbps +// wait(0.0051); +// 20bit/500kbps +// wait_us(40); +} + +/* CAN1受信関数 */ +void CAN1_Rcv(void) +{ + can1.read(Rcv_Msg); + if( Rcv_Msg.id == 0x7DF ) { + if( Rcv_Msg.data[0] == 0x01 ) { +// Module1Encoder = 0; + Module1Encoder += ( Rcv_Msg.data[1] << 8 ); + Module1Encoder += Rcv_Msg.data[2]; + Module1.Set_EncoderPulse( Module1Encoder ); + } else if( Rcv_Msg.data[0] == 0x03 ) { +// Module2Encoder = 0; + Module2Encoder += ( Rcv_Msg.data[1] << 8 ); + Module2Encoder += Rcv_Msg.data[2]; + Module2.Set_EncoderPulse( Module2Encoder ); + } else if( Rcv_Msg.data[0] == 0x06 ) { + LiftEncoder += ( Rcv_Msg.data[1] << 8 ); + LiftEncoder += Rcv_Msg.data[2]; + } + } +} + +/* GUI受信割込み関数 */ +void GUI_Receive(void) +{ + GUIData = RaspPi.getc(); +// if( GUIData != 0x00 && timer.read_ms() <= 200) { + if( GUIData != 0x00 ) { + Ps3_Led = 0; + Gui_Led = 1; + GUI_Safety_Timer.attach(&GUI_Safety_Stop, 0.1); + if(GUIData == 0x7F) { + EmgFlag = true; + RaspPi.printf("EMG:True"); + } else if(GUIData == 0x80) { + EmgFlag = false; + RaspPi.printf("EMG:False"); + } + ControlMode = GUIMODE; + RaspPi.printf("GUImode"); + } else { + Ps3_Led = 1; + Gui_Led = 0; + GUI_Safety_Timer.detach(); + ControlMode = PS3MODE; + Module1Status = MANUAL_STATUS; + Module2Status = MANUAL_STATUS; + RobotStatus = MANUAL_STATUS; + RaspPi.printf("PS3mode "); + } + RaspPi.printf("GUIData:0x%x\n",GUIData); +} + + +/* GUIタイムアウト停止 */ +void GUI_Safety_Stop(void) +{ + //未実装 +} + +/* 非常停止ブザー制御関数 */ +void Buzzer_Toggle(void) +{ + if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) { + buzzer = !buzzer; + } else { + buzzer = 0; + } +} + +/* 非常停止判定関数 */ +void Emg_Stop(void) +{ + Start = ps3.getSTARTState(); + Select = ps3.getSELECTState(); + if( (Select == 1 || EmgFlag == true) || ( emg_stop_sw == 1 ) ) { + //停止 + Emg_Led = 1; + emg_stop_tri = 0; + } else if( ( Start == 1 || EmgFlag == false) && ( emg_stop_sw == 0 ) ) { + //通常 + Emg_Led = 0; + emg_stop_tri = 1; + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 26 00:35:42 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file