File content as of revision 2:8b32f6901e10:

/*************************************************************************************************

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");
    }
}
***************************************************************************************************/