#define MOTOR_DRIVE_VERSION 6 ↓ #define MOTOR_DRIVE_VERSION 5 TIMEOUT_VALUE 1.0f ↓ TIMEOUT_VALUE 10.0f ↓
Dependencies: mbed can_network_definition_2022 bitCommunication_Ver2 motorDrive_LAP PID QEI_hw LPC1549QEI LPC1549_EEPROM motorDrive_SMB MedianFilter2020 buzzer USBDevice
main.cpp@9:c1dc00ede75d, 2022-08-24 (annotated)
- Committer:
- hoshi_t
- Date:
- Wed Aug 24 12:54:02 2022 +0000
- Revision:
- 9:c1dc00ede75d
- Parent:
- 8:f6851c48f94b
jsiojoijrt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YutaTogashi | 2:40528aee5ebe | 1 | /*************************************** |
YutaTogashi | 2:40528aee5ebe | 2 | MotorDriver Ver4 or Ver5 or Ver6 select |
YutaTogashi | 2:40528aee5ebe | 3 | ***************************************/ |
YutaTogashi | 5:58c684e2ee47 | 4 | //<注意>プログラム上でのバージョンです 書き込む基板に合わせてここだけ変更してください. |
YutaTogashi | 5:58c684e2ee47 | 5 | //Ver4 2019Bチーム手動機で使っていたMD(CAN通信のコネクタとPWM入力用のコネクタとロータリーエンコーダ入力用のコネクタがあります 全部でコネクタが4つ) |
YutaTogashi | 5:58c684e2ee47 | 6 | //Ver5 2019Bチーム自動機で使っていたMD(CAN通信のコネクタとロータリーエンコーダ入力用のコネクタがあります 全部でコネクタが3つ) |
YutaTogashi | 2:40528aee5ebe | 7 | //Ver6 センサ入力に対応したMD |
hoshi_t | 9:c1dc00ede75d | 8 | #define MOTOR_DRIVER_VERSION 5 |
YutaTogashi | 2:40528aee5ebe | 9 | /*************************************** |
YutaTogashi | 2:40528aee5ebe | 10 | SMB or LAP select |
YutaTogashi | 2:40528aee5ebe | 11 | ***************************************/ |
YutaTogashi | 2:40528aee5ebe | 12 | //駆動方式の選択 選択する方のコメントアウトを外してください. |
YutaTogashi | 2:40528aee5ebe | 13 | //<default> SMBモード |
YutaTogashi | 2:40528aee5ebe | 14 | //<option> LAPモード |
YutaTogashi | 2:40528aee5ebe | 15 | #define MOTOR_DRIVER_MODE_SMB |
YutaTogashi | 2:40528aee5ebe | 16 | //#define MOTOR_DRIVER_MODE_LAP |
YutaTogashi | 2:40528aee5ebe | 17 | /*************************************************************************************************************************/ |
YutaTogashi | 2:40528aee5ebe | 18 | // souce code |
YutaTogashi | 2:40528aee5ebe | 19 | /*************************** |
YutaTogashi | 2:40528aee5ebe | 20 | include file |
YutaTogashi | 2:40528aee5ebe | 21 | ***************************/ |
YutaTogashi | 0:4fe08f484460 | 22 | #include "mbed.h" |
YutaTogashi | 4:6e88615c830b | 23 | #include "can_network_motordriver_definition.h" |
YutaTogashi | 8:f6851c48f94b | 24 | #include "Can_network_definition.h" |
YutaTogashi | 0:4fe08f484460 | 25 | #include "bitCommunication.h" |
YutaTogashi | 0:4fe08f484460 | 26 | #include "buzzer.h" |
YutaTogashi | 0:4fe08f484460 | 27 | #include "eeprom.h" |
YutaTogashi | 0:4fe08f484460 | 28 | #include "eeprom_initalize.h" |
YutaTogashi | 4:6e88615c830b | 29 | //#include "QEI.h" |
YutaTogashi | 4:6e88615c830b | 30 | #include "qeihw.h" |
YutaTogashi | 0:4fe08f484460 | 31 | #include "Pid.h" |
YutaTogashi | 0:4fe08f484460 | 32 | #include "motorDrive_SMB.h" |
YutaTogashi | 0:4fe08f484460 | 33 | #include "motorDrive_LAP.h" |
YutaTogashi | 0:4fe08f484460 | 34 | #include "MedianFilter.h" |
YutaTogashi | 0:4fe08f484460 | 35 | #include "definition.h" |
YutaTogashi | 0:4fe08f484460 | 36 | /****************************** |
YutaTogashi | 0:4fe08f484460 | 37 | definition instance |
YutaTogashi | 0:4fe08f484460 | 38 | ******************************/ |
YutaTogashi | 0:4fe08f484460 | 39 | CAN can(CAN_PIN[CAN_RD],CAN_PIN[CAN_TD]); |
YutaTogashi | 0:4fe08f484460 | 40 | Serial pc(SERIAL_PIN[SERIAL_TX],SERIAL_PIN[SERIAL_RX],communication_baud::PC); |
YutaTogashi | 0:4fe08f484460 | 41 | BusIn idSwitch(ID_PIN[SWITCH1],ID_PIN[SWITCH2],ID_PIN[SWITCH3],ID_PIN[SWITCH4]); |
YutaTogashi | 4:6e88615c830b | 42 | //QEI encoder(ENCODER_PIN[ENCODER_A],ENCODER_PIN[ENCODER_B],NC,ENCODER_PPR); |
YutaTogashi | 4:6e88615c830b | 43 | QEIHW qei(QEI_DIRINV_NONE,QEI_SIGNALMODE_QUAD,QEI_CAPMODE_4X,QEI_INVINX_NONE); //QEI_hw |
YutaTogashi | 0:4fe08f484460 | 44 | buzzer led(LED_PIN[FREE]); |
YutaTogashi | 0:4fe08f484460 | 45 | PwmOut pwmLed[2] = { |
YutaTogashi | 0:4fe08f484460 | 46 | PwmOut(LED_PIN[CW]), |
YutaTogashi | 0:4fe08f484460 | 47 | PwmOut(LED_PIN[CCW]), |
YutaTogashi | 0:4fe08f484460 | 48 | }; |
YutaTogashi | 0:4fe08f484460 | 49 | Pid pid; |
YutaTogashi | 0:4fe08f484460 | 50 | Ticker controlCycle; |
YutaTogashi | 0:4fe08f484460 | 51 | Timer errorTimer; |
YutaTogashi | 4:6e88615c830b | 52 | #ifdef MOTOR_DRIVER_MODE_LAP |
YutaTogashi | 2:40528aee5ebe | 53 | motorDriveLAP motor(A3921_PIN[PWMH],A3921_PIN[PHASE]); |
YutaTogashi | 2:40528aee5ebe | 54 | #else |
YutaTogashi | 2:40528aee5ebe | 55 | motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]); |
YutaTogashi | 2:40528aee5ebe | 56 | #endif |
YutaTogashi | 2:40528aee5ebe | 57 | #if MOTOR_DRIVER_VERSION == 4 |
YutaTogashi | 2:40528aee5ebe | 58 | DigitalOut pwml(PWML_PIN,true); |
YutaTogashi | 2:40528aee5ebe | 59 | #endif |
YutaTogashi | 0:4fe08f484460 | 60 | /****************************** |
YutaTogashi | 0:4fe08f484460 | 61 | global variable |
YutaTogashi | 0:4fe08f484460 | 62 | ******************************/ |
YutaTogashi | 0:4fe08f484460 | 63 | float parameter[PARAMETER_NUMBER] = {0.0f,0.0f,0.0f,0.0f}; |
YutaTogashi | 0:4fe08f484460 | 64 | float outputDuty = 0.0f; |
YutaTogashi | 0:4fe08f484460 | 65 | float targetRpm = 0.0f; |
YutaTogashi | 4:6e88615c830b | 66 | float rps = 0.0f; |
YutaTogashi | 4:6e88615c830b | 67 | float rpm = 0.0f; |
YutaTogashi | 4:6e88615c830b | 68 | float rotation[2] = {0.0f,0.0f}; |
YutaTogashi | 0:4fe08f484460 | 69 | int powerFilterCount; |
YutaTogashi | 0:4fe08f484460 | 70 | int powerFilterArray[10]; |
YutaTogashi | 3:097710997f6b | 71 | bool enable_motor_output = false; //モーターへの出力許可を管理(true : 出力可能 , false : 出力を強制停止) |
YutaTogashi | 0:4fe08f484460 | 72 | bool outputMode = false; |
YutaTogashi | 0:4fe08f484460 | 73 | bool printfMode = false; |
YutaTogashi | 0:4fe08f484460 | 74 | bool receiveParameterStatus[4] = {false,false,false,false}; |
YutaTogashi | 0:4fe08f484460 | 75 | /****************************** |
YutaTogashi | 0:4fe08f484460 | 76 | definition function |
YutaTogashi | 0:4fe08f484460 | 77 | ******************************/ |
YutaTogashi | 0:4fe08f484460 | 78 | void initalize(); |
YutaTogashi | 0:4fe08f484460 | 79 | void mainLoop(); |
YutaTogashi | 3:097710997f6b | 80 | void outputIndicator(float control_duty); |
YutaTogashi | 3:097710997f6b | 81 | void safetyControl(float duty); |
YutaTogashi | 0:4fe08f484460 | 82 | void canReceiveHandler(); |
YutaTogashi | 0:4fe08f484460 | 83 | void debag(); |
YutaTogashi | 0:4fe08f484460 | 84 | /****************************** |
YutaTogashi | 0:4fe08f484460 | 85 | function content |
YutaTogashi | 0:4fe08f484460 | 86 | ******************************/ |
YutaTogashi | 0:4fe08f484460 | 87 | int main() { |
YutaTogashi | 0:4fe08f484460 | 88 | initalize(); |
YutaTogashi | 0:4fe08f484460 | 89 | while(1){ |
YutaTogashi | 0:4fe08f484460 | 90 | } |
YutaTogashi | 0:4fe08f484460 | 91 | } |
YutaTogashi | 0:4fe08f484460 | 92 | |
YutaTogashi | 0:4fe08f484460 | 93 | void initalize() { |
YutaTogashi | 4:6e88615c830b | 94 | /***************************EEPROM 読み取り処理*******************************/ |
YutaTogashi | 0:4fe08f484460 | 95 | can_flame eepromData; |
YutaTogashi | 0:4fe08f484460 | 96 | Chip_EEPROM_init(); |
YutaTogashi | 0:4fe08f484460 | 97 | __disable_irq(); |
YutaTogashi | 0:4fe08f484460 | 98 | read_eeprom((char*)0, eepromData.c, 4); |
YutaTogashi | 0:4fe08f484460 | 99 | parameter[KP] = eepromData.f; |
YutaTogashi | 0:4fe08f484460 | 100 | read_eeprom((char*)4, eepromData.c, 4); |
YutaTogashi | 0:4fe08f484460 | 101 | parameter[KI] = eepromData.f; |
YutaTogashi | 0:4fe08f484460 | 102 | read_eeprom((char*)8, eepromData.c, 4); |
YutaTogashi | 0:4fe08f484460 | 103 | parameter[KD] = eepromData.f; |
YutaTogashi | 0:4fe08f484460 | 104 | __enable_irq(); |
YutaTogashi | 4:6e88615c830b | 105 | /***************************************************************************/ |
YutaTogashi | 4:6e88615c830b | 106 | |
YutaTogashi | 4:6e88615c830b | 107 | /******************************割り込み優先度設定******************************/ |
YutaTogashi | 2:40528aee5ebe | 108 | NVIC_SetPriority(C_CAN0_IRQn,0); |
YutaTogashi | 2:40528aee5ebe | 109 | NVIC_SetPriority(QEI_IRQn,1); |
YutaTogashi | 4:6e88615c830b | 110 | /***************************************************************************/ |
YutaTogashi | 0:4fe08f484460 | 111 | |
YutaTogashi | 4:6e88615c830b | 112 | /*******************************CAN通信の設定*********************************/ |
YutaTogashi | 0:4fe08f484460 | 113 | can.frequency(communication_baud::CAN); |
YutaTogashi | 2:40528aee5ebe | 114 | can.attach(&canReceiveHandler,CAN::RxIrq); |
YutaTogashi | 4:6e88615c830b | 115 | /***************************************************************************/ |
YutaTogashi | 2:40528aee5ebe | 116 | |
YutaTogashi | 4:6e88615c830b | 117 | /***************************MotorDrive 設定**********************************/ |
YutaTogashi | 0:4fe08f484460 | 118 | motor.setupFrequency(motor_option::FREQUENCY); |
YutaTogashi | 0:4fe08f484460 | 119 | motor.setupLimitDuty(motor_option::LOWER_LIMIT,motor_option::UPPER_LIMIT); |
YutaTogashi | 4:6e88615c830b | 120 | /****************************************************************************/ |
YutaTogashi | 4:6e88615c830b | 121 | |
YutaTogashi | 4:6e88615c830b | 122 | /*********************************SpeedPID設定*******************************/ |
YutaTogashi | 0:4fe08f484460 | 123 | pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE); |
YutaTogashi | 4:6e88615c830b | 124 | /***************************************************************************/ |
YutaTogashi | 0:4fe08f484460 | 125 | |
YutaTogashi | 4:6e88615c830b | 126 | /**********************************その他************************************/ |
YutaTogashi | 4:6e88615c830b | 127 | qei.SetDigiFilter(480UL); //QEI_hw filter設定 |
YutaTogashi | 4:6e88615c830b | 128 | qei.SetMaxPosition(0xFFFFFFFF); //QEI_hw filter設定 |
YutaTogashi | 4:6e88615c830b | 129 | qei.SetVelocityTimerReload_us(10000); //QEI_hw filter設定 |
YutaTogashi | 4:6e88615c830b | 130 | |
YutaTogashi | 4:6e88615c830b | 131 | errorTimer.start(); //CAN通信 タイムアウト検知用計測開始 |
YutaTogashi | 5:58c684e2ee47 | 132 | pc.printf("\r\nInitalize MotorDriver!\r\n"); //initalize終了出力 |
YutaTogashi | 5:58c684e2ee47 | 133 | pc.printf("Firmware:robocon2021_TBCMotorDriver( https://os.mbed.com/users/YutaTogashi/code/robocon2021_TBCMotorDriver/ )\r\n"); //Firmwareをシリアル出力 |
YutaTogashi | 5:58c684e2ee47 | 134 | pc.printf("UsingLibrary:can_network_motordriver ( https://os.mbed.com/users/YutaTogashi/code/can_network_motordriver/ )\r\n"); //MDを使用しやすくしたライブラリ シリアル出力 |
YutaTogashi | 7:b91354ec59bd | 135 | #if MOTOR_DRIVER_VERSION == 4 |
YutaTogashi | 7:b91354ec59bd | 136 | pc.printf("MotorDriverVersion:4\r\n"); |
YutaTogashi | 7:b91354ec59bd | 137 | #elif MOTOR_DRIVER_VERSION == 5 |
YutaTogashi | 7:b91354ec59bd | 138 | pc.printf("MotorDriverVersion:5\r\n"); |
YutaTogashi | 7:b91354ec59bd | 139 | #elif MOTOR_DRIVER_VERSION == 6 |
YutaTogashi | 7:b91354ec59bd | 140 | pc.printf("MotorDriverVersion:6\r\n"); |
YutaTogashi | 7:b91354ec59bd | 141 | #endif |
YutaTogashi | 4:6e88615c830b | 142 | controlCycle.attach(&mainLoop,period::CONTROL_CYCLE); //制御ループ設定 |
YutaTogashi | 4:6e88615c830b | 143 | /***************************************************************************/ |
YutaTogashi | 0:4fe08f484460 | 144 | } |
YutaTogashi | 0:4fe08f484460 | 145 | |
YutaTogashi | 2:40528aee5ebe | 146 | void mainLoop() { |
YutaTogashi | 4:6e88615c830b | 147 | //RPS,RPMの計算 |
YutaTogashi | 4:6e88615c830b | 148 | rotation[0] = static_cast<float>(static_cast<int32_t>(qei.GetPosition())) / (static_cast<float>(ENCODER_PPR) * 4.0f); |
YutaTogashi | 4:6e88615c830b | 149 | rps = (rotation[0] - rotation[1]) / period::CONTROL_CYCLE; |
YutaTogashi | 4:6e88615c830b | 150 | rpm = rps * 60.0f; |
YutaTogashi | 4:6e88615c830b | 151 | rotation[1] = rotation[0]; |
YutaTogashi | 4:6e88615c830b | 152 | |
YutaTogashi | 4:6e88615c830b | 153 | //速度PIDモードの挙動 |
YutaTogashi | 0:4fe08f484460 | 154 | if(outputMode) { |
YutaTogashi | 0:4fe08f484460 | 155 | pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE); |
YutaTogashi | 4:6e88615c830b | 156 | pid.calculate(targetRpm,/*encoder.getRPM()*/rpm); |
YutaTogashi | 0:4fe08f484460 | 157 | outputDuty = pid.getDuty(); |
YutaTogashi | 0:4fe08f484460 | 158 | } |
YutaTogashi | 0:4fe08f484460 | 159 | |
YutaTogashi | 4:6e88615c830b | 160 | //TIMEOUT判定 |
YutaTogashi | 3:097710997f6b | 161 | if(errorTimer.read() > TIMEOUT_VALUE) enable_motor_output = false; |
YutaTogashi | 3:097710997f6b | 162 | else enable_motor_output = true; |
YutaTogashi | 3:097710997f6b | 163 | |
YutaTogashi | 4:6e88615c830b | 164 | //出力処理 |
YutaTogashi | 3:097710997f6b | 165 | if(enable_motor_output) { |
YutaTogashi | 4:6e88615c830b | 166 | led.output(outputMode); //速度PIDモードの場合は,LEDインジゲータが点灯 |
YutaTogashi | 3:097710997f6b | 167 | } else { |
YutaTogashi | 4:6e88615c830b | 168 | led.output(0.5f); //TIMEOUT時は,LEDインジゲータが点滅 |
YutaTogashi | 4:6e88615c830b | 169 | safetyControl(0.05f); //徐々に出力を下げる |
YutaTogashi | 4:6e88615c830b | 170 | pid.reset(); //速度PIDの計算をリセット |
YutaTogashi | 3:097710997f6b | 171 | } |
YutaTogashi | 2:40528aee5ebe | 172 | |
YutaTogashi | 4:6e88615c830b | 173 | outputIndicator(outputDuty); //Duty,Phaseインジゲータを制御 |
YutaTogashi | 4:6e88615c830b | 174 | motor.output(outputDuty); //A3921制御 |
YutaTogashi | 4:6e88615c830b | 175 | |
YutaTogashi | 4:6e88615c830b | 176 | debag(); //デバッグ関数 |
YutaTogashi | 0:4fe08f484460 | 177 | } |
YutaTogashi | 0:4fe08f484460 | 178 | |
YutaTogashi | 0:4fe08f484460 | 179 | void debag() { |
YutaTogashi | 0:4fe08f484460 | 180 | if(!(printfMode)) { |
YutaTogashi | 0:4fe08f484460 | 181 | pc.printf("SWITCH:%d\t",idSwitch.read()); |
YutaTogashi | 8:f6851c48f94b | 182 | pc.printf("ID:%d\t", (idSwitch.read())); |
YutaTogashi | 0:4fe08f484460 | 183 | pc.printf("DUTY:%.2f\t",outputDuty); |
YutaTogashi | 4:6e88615c830b | 184 | pc.printf("RPM:%.2f\t",/*encoder.getRPM()*/rpm); |
YutaTogashi | 0:4fe08f484460 | 185 | |
YutaTogashi | 0:4fe08f484460 | 186 | if(outputMode) { |
YutaTogashi | 0:4fe08f484460 | 187 | pc.printf("TARGET:%.1f\t",targetRpm); |
YutaTogashi | 0:4fe08f484460 | 188 | pc.printf("Kp:%f\t",parameter[KP]); |
YutaTogashi | 0:4fe08f484460 | 189 | pc.printf("Ki:%f\t",parameter[KI]); |
YutaTogashi | 0:4fe08f484460 | 190 | pc.printf("Kd:%f\t",parameter[KD]); |
YutaTogashi | 0:4fe08f484460 | 191 | } |
YutaTogashi | 0:4fe08f484460 | 192 | pc.printf("TIME:%f",errorTimer.read()); |
YutaTogashi | 0:4fe08f484460 | 193 | pc.printf("\r\n"); |
YutaTogashi | 0:4fe08f484460 | 194 | } else { |
YutaTogashi | 6:a3efeb48498a | 195 | pc.printf("%f,",targetRpm); |
YutaTogashi | 4:6e88615c830b | 196 | pc.printf("%f,",/*encoder.getRPM()*/rpm); |
YutaTogashi | 6:a3efeb48498a | 197 | pc.printf("%f",outputDuty); |
YutaTogashi | 0:4fe08f484460 | 198 | pc.printf("\r\n"); |
YutaTogashi | 0:4fe08f484460 | 199 | } |
YutaTogashi | 0:4fe08f484460 | 200 | } |
YutaTogashi | 0:4fe08f484460 | 201 | |
YutaTogashi | 2:40528aee5ebe | 202 | void outputIndicator(float duty) { |
YutaTogashi | 2:40528aee5ebe | 203 | if(fabs(duty) > 0.0f) { |
YutaTogashi | 2:40528aee5ebe | 204 | if(duty > 0.0f) { |
YutaTogashi | 2:40528aee5ebe | 205 | pwmLed[CW].write(fabs(duty)); |
YutaTogashi | 0:4fe08f484460 | 206 | pwmLed[CCW].write(0.0f); |
YutaTogashi | 0:4fe08f484460 | 207 | } else { |
YutaTogashi | 0:4fe08f484460 | 208 | pwmLed[CW].write(0.0f); |
YutaTogashi | 2:40528aee5ebe | 209 | pwmLed[CCW].write(fabs(duty)); |
YutaTogashi | 0:4fe08f484460 | 210 | } |
YutaTogashi | 0:4fe08f484460 | 211 | } else { |
YutaTogashi | 0:4fe08f484460 | 212 | pwmLed[CW].write(0.0f); |
YutaTogashi | 0:4fe08f484460 | 213 | pwmLed[CCW].write(0.0f); |
YutaTogashi | 0:4fe08f484460 | 214 | } |
YutaTogashi | 0:4fe08f484460 | 215 | } |
YutaTogashi | 0:4fe08f484460 | 216 | |
YutaTogashi | 3:097710997f6b | 217 | void safetyControl(float control_duty) { |
YutaTogashi | 4:6e88615c830b | 218 | if(fabs(outputDuty - 0.0f) > 0.10f) { |
YutaTogashi | 3:097710997f6b | 219 | if(outputDuty > 0.0f) outputDuty -= control_duty; |
YutaTogashi | 3:097710997f6b | 220 | else outputDuty += control_duty; |
YutaTogashi | 3:097710997f6b | 221 | } else { |
YutaTogashi | 3:097710997f6b | 222 | outputDuty = 0.0f; |
YutaTogashi | 3:097710997f6b | 223 | } |
YutaTogashi | 3:097710997f6b | 224 | } |
YutaTogashi | 3:097710997f6b | 225 | |
YutaTogashi | 0:4fe08f484460 | 226 | void canReceiveHandler() { |
YutaTogashi | 0:4fe08f484460 | 227 | can_flame message,returnMessage; |
YutaTogashi | 0:4fe08f484460 | 228 | CANMessage sendMsg,receiveMessage; |
YutaTogashi | 0:4fe08f484460 | 229 | |
YutaTogashi | 8:f6851c48f94b | 230 | float value = 0.0f; |
YutaTogashi | 8:f6851c48f94b | 231 | CANMessage msg; |
YutaTogashi | 8:f6851c48f94b | 232 | can.read(msg); |
YutaTogashi | 0:4fe08f484460 | 233 | |
YutaTogashi | 8:f6851c48f94b | 234 | if((getCanIdData(msg.id,0) == can_protcol::device_type::MOTOR_DRIVER) && (getCanIdData(msg.id,1) == idSwitch.read())) { //MD基板宛のメッセージのみ抽出 |
YutaTogashi | 8:f6851c48f94b | 235 | errorTimer.reset(); //TIMEOUT時間管理用タイマーをリセット |
YutaTogashi | 8:f6851c48f94b | 236 | switch (((static_cast<uint8_t>(msg.data[0])) >> 4)) { |
YutaTogashi | 8:f6851c48f94b | 237 | case can_protcol::device_type::CONTROL_MCU: //メイン制御基板からきたデータを抽出 |
YutaTogashi | 8:f6851c48f94b | 238 | case can_protcol::device_type::PC: //PCからきたデータを抽出 |
YutaTogashi | 8:f6851c48f94b | 239 | switch (getCanIdData(msg.id,2)) { //DataTypeを確認 |
YutaTogashi | 8:f6851c48f94b | 240 | case can_protcol::data_type::motor_driver::DEBAG_MODE: //debag mode |
YutaTogashi | 8:f6851c48f94b | 241 | break; |
YutaTogashi | 8:f6851c48f94b | 242 | case can_protcol::data_type::motor_driver::PARAMETER: //パラメータ受信 |
YutaTogashi | 8:f6851c48f94b | 243 | switch (static_cast<uint8_t>(msg.data[1])) { |
YutaTogashi | 8:f6851c48f94b | 244 | case 0: //PositionPid Kp |
YutaTogashi | 8:f6851c48f94b | 245 | break; |
YutaTogashi | 8:f6851c48f94b | 246 | case 1: //PositoinPid Ki |
YutaTogashi | 8:f6851c48f94b | 247 | break; |
YutaTogashi | 8:f6851c48f94b | 248 | case 2: //PositionPid Kd |
YutaTogashi | 8:f6851c48f94b | 249 | break; |
YutaTogashi | 8:f6851c48f94b | 250 | case 3: //VelocityPid Kp |
YutaTogashi | 8:f6851c48f94b | 251 | memcpy(&value,msg.data+2,4); |
YutaTogashi | 8:f6851c48f94b | 252 | parameter[KP] = value; |
YutaTogashi | 8:f6851c48f94b | 253 | receiveParameterStatus[0] = true; |
YutaTogashi | 8:f6851c48f94b | 254 | break; |
YutaTogashi | 8:f6851c48f94b | 255 | case 4: //VelocityPid Ki |
YutaTogashi | 8:f6851c48f94b | 256 | memcpy(&value,msg.data+2,4); |
YutaTogashi | 8:f6851c48f94b | 257 | parameter[KI] = value; |
YutaTogashi | 8:f6851c48f94b | 258 | receiveParameterStatus[1] = true; |
YutaTogashi | 8:f6851c48f94b | 259 | break; |
YutaTogashi | 8:f6851c48f94b | 260 | case 5: //VelocityPid Kd |
YutaTogashi | 8:f6851c48f94b | 261 | memcpy(&value,msg.data+2,4); |
YutaTogashi | 8:f6851c48f94b | 262 | parameter[KD] = value; |
YutaTogashi | 8:f6851c48f94b | 263 | receiveParameterStatus[2] = true; |
YutaTogashi | 8:f6851c48f94b | 264 | break; |
YutaTogashi | 8:f6851c48f94b | 265 | case 6: //Acceleration |
YutaTogashi | 8:f6851c48f94b | 266 | break; |
YutaTogashi | 8:f6851c48f94b | 267 | case 7: //Timeout Time |
YutaTogashi | 8:f6851c48f94b | 268 | break; |
YutaTogashi | 8:f6851c48f94b | 269 | case 8: //Max Duty |
YutaTogashi | 8:f6851c48f94b | 270 | break; |
YutaTogashi | 8:f6851c48f94b | 271 | case 9: //Max Velocity(RPM) |
YutaTogashi | 8:f6851c48f94b | 272 | break; |
YutaTogashi | 8:f6851c48f94b | 273 | case 10: //Encoder PPR |
YutaTogashi | 8:f6851c48f94b | 274 | break; |
YutaTogashi | 8:f6851c48f94b | 275 | case 11: //AbsEncoder Reset |
YutaTogashi | 8:f6851c48f94b | 276 | break; |
YutaTogashi | 8:f6851c48f94b | 277 | default: |
YutaTogashi | 8:f6851c48f94b | 278 | break; |
YutaTogashi | 8:f6851c48f94b | 279 | } |
YutaTogashi | 8:f6851c48f94b | 280 | break; |
YutaTogashi | 8:f6851c48f94b | 281 | case can_protcol::data_type::motor_driver::CONTROL_INFORMATION: //制御量受信 |
YutaTogashi | 8:f6851c48f94b | 282 | switch (static_cast<uint8_t>(msg.data[1])) { |
YutaTogashi | 8:f6851c48f94b | 283 | case 0: //duty駆動 |
YutaTogashi | 8:f6851c48f94b | 284 | memcpy(&outputDuty,msg.data+2,4); |
YutaTogashi | 8:f6851c48f94b | 285 | outputMode = false; |
YutaTogashi | 8:f6851c48f94b | 286 | break; |
YutaTogashi | 8:f6851c48f94b | 287 | case 1: //速度制御 |
YutaTogashi | 8:f6851c48f94b | 288 | if((static_cast<uint8_t>(msg.data[6]) & 0b1) == 0b1) { //駆動電源のフィードバック情報より駆動電源が入力されている場合の挙動 |
YutaTogashi | 8:f6851c48f94b | 289 | memcpy(&targetRpm,msg.data+2,4); |
YutaTogashi | 8:f6851c48f94b | 290 | outputMode = true; |
YutaTogashi | 8:f6851c48f94b | 291 | } else { //駆動電源のフィードバック情報より駆動電源が入力されていない場合の挙動 |
YutaTogashi | 8:f6851c48f94b | 292 | targetRpm = 0.0f; |
YutaTogashi | 8:f6851c48f94b | 293 | outputDuty = 0.0f; |
YutaTogashi | 8:f6851c48f94b | 294 | pid.reset(); |
YutaTogashi | 8:f6851c48f94b | 295 | outputMode = false; |
YutaTogashi | 8:f6851c48f94b | 296 | } |
YutaTogashi | 8:f6851c48f94b | 297 | break; |
YutaTogashi | 8:f6851c48f94b | 298 | default: |
YutaTogashi | 8:f6851c48f94b | 299 | break; |
YutaTogashi | 8:f6851c48f94b | 300 | } |
YutaTogashi | 8:f6851c48f94b | 301 | break; |
YutaTogashi | 8:f6851c48f94b | 302 | case can_protcol::data_type::motor_driver::STEER_CONTROL_INFORMATION: //(ステア用)制御量受信 |
YutaTogashi | 8:f6851c48f94b | 303 | break; |
YutaTogashi | 8:f6851c48f94b | 304 | case can_protcol::data_type::motor_driver::PRINTF_FORMAT: //printf出力形式の指定情報を受信 |
YutaTogashi | 8:f6851c48f94b | 305 | break; |
YutaTogashi | 8:f6851c48f94b | 306 | default: |
YutaTogashi | 8:f6851c48f94b | 307 | break; |
YutaTogashi | 8:f6851c48f94b | 308 | } |
YutaTogashi | 8:f6851c48f94b | 309 | break; |
YutaTogashi | 8:f6851c48f94b | 310 | default: |
YutaTogashi | 8:f6851c48f94b | 311 | break; |
YutaTogashi | 8:f6851c48f94b | 312 | } |
YutaTogashi | 8:f6851c48f94b | 313 | } |
YutaTogashi | 8:f6851c48f94b | 314 | |
YutaTogashi | 8:f6851c48f94b | 315 | |
YutaTogashi | 8:f6851c48f94b | 316 | //sendMsg.id = MAIN_MCU; |
YutaTogashi | 0:4fe08f484460 | 317 | |
YutaTogashi | 8:f6851c48f94b | 318 | /*if((MD_0_MCU + idSwitch.read()) == receiveMessage.id) { |
YutaTogashi | 0:4fe08f484460 | 319 | errorTimer.reset(); |
YutaTogashi | 0:4fe08f484460 | 320 | switch ((int)receiveMessage.data[0]) { |
YutaTogashi | 0:4fe08f484460 | 321 | case md_can_flame::SEND_KP: |
YutaTogashi | 0:4fe08f484460 | 322 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 323 | __disable_irq(); |
YutaTogashi | 0:4fe08f484460 | 324 | write_eeprom(message.c,(char*)0,4); |
YutaTogashi | 0:4fe08f484460 | 325 | __enable_irq(); |
YutaTogashi | 0:4fe08f484460 | 326 | parameter[KP] = message.f; |
YutaTogashi | 0:4fe08f484460 | 327 | receiveParameterStatus[0] = true; |
YutaTogashi | 0:4fe08f484460 | 328 | break; |
YutaTogashi | 0:4fe08f484460 | 329 | case md_can_flame::SEND_KI: |
YutaTogashi | 0:4fe08f484460 | 330 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 331 | __disable_irq(); |
YutaTogashi | 0:4fe08f484460 | 332 | write_eeprom(message.c,(char*)4,4); |
YutaTogashi | 0:4fe08f484460 | 333 | __enable_irq(); |
YutaTogashi | 0:4fe08f484460 | 334 | parameter[KI] = message.f; |
YutaTogashi | 0:4fe08f484460 | 335 | receiveParameterStatus[1] = true; |
YutaTogashi | 0:4fe08f484460 | 336 | break; |
YutaTogashi | 0:4fe08f484460 | 337 | case md_can_flame::SEND_KD: |
YutaTogashi | 0:4fe08f484460 | 338 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 339 | __disable_irq(); |
YutaTogashi | 0:4fe08f484460 | 340 | write_eeprom(message.c,(char*)8,4); |
YutaTogashi | 0:4fe08f484460 | 341 | __enable_irq(); |
YutaTogashi | 0:4fe08f484460 | 342 | parameter[KD] = message.f; |
YutaTogashi | 0:4fe08f484460 | 343 | receiveParameterStatus[2] = true; |
YutaTogashi | 0:4fe08f484460 | 344 | break; |
YutaTogashi | 0:4fe08f484460 | 345 | case md_can_flame::SEND_ACCELERATION: |
YutaTogashi | 0:4fe08f484460 | 346 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 347 | parameter[ACCELERATION] = message.f; |
YutaTogashi | 0:4fe08f484460 | 348 | receiveParameterStatus[3] = true; |
YutaTogashi | 0:4fe08f484460 | 349 | break; |
YutaTogashi | 0:4fe08f484460 | 350 | case md_can_flame::SEND_CONTROL_DUTY: |
YutaTogashi | 0:4fe08f484460 | 351 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 352 | outputDuty = message.f; |
YutaTogashi | 0:4fe08f484460 | 353 | outputMode = false; |
YutaTogashi | 0:4fe08f484460 | 354 | break; |
YutaTogashi | 0:4fe08f484460 | 355 | case md_can_flame::SEND_CONTROL_RPM: |
YutaTogashi | 0:4fe08f484460 | 356 | if(bitRead((int)receiveMessage.data[1],0)) { |
YutaTogashi | 0:4fe08f484460 | 357 | for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i]; |
YutaTogashi | 0:4fe08f484460 | 358 | targetRpm = message.f; |
YutaTogashi | 0:4fe08f484460 | 359 | outputMode = true; |
YutaTogashi | 0:4fe08f484460 | 360 | } else { |
YutaTogashi | 0:4fe08f484460 | 361 | targetRpm = 0.0f; |
YutaTogashi | 0:4fe08f484460 | 362 | outputDuty = 0.0f; |
YutaTogashi | 0:4fe08f484460 | 363 | pid.reset(); |
YutaTogashi | 0:4fe08f484460 | 364 | outputMode = false; |
YutaTogashi | 0:4fe08f484460 | 365 | } |
YutaTogashi | 0:4fe08f484460 | 366 | break; |
YutaTogashi | 0:4fe08f484460 | 367 | case md_can_flame::REQUEST_RECEIVE_CURRENT: |
YutaTogashi | 0:4fe08f484460 | 368 | //no code |
YutaTogashi | 0:4fe08f484460 | 369 | break; |
YutaTogashi | 0:4fe08f484460 | 370 | case md_can_flame::REQUEST_RECEIVE_ERROR: |
YutaTogashi | 0:4fe08f484460 | 371 | int parameterStatus = 0; |
YutaTogashi | 0:4fe08f484460 | 372 | for(int i=0;i<4;i++) bitWrite(¶meterStatus,i,receiveParameterStatus[i]); |
YutaTogashi | 0:4fe08f484460 | 373 | sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ERROR; |
YutaTogashi | 0:4fe08f484460 | 374 | sendMsg.data[2] = parameterStatus; |
YutaTogashi | 0:4fe08f484460 | 375 | can.write(sendMsg); |
YutaTogashi | 0:4fe08f484460 | 376 | break; |
YutaTogashi | 4:6e88615c830b | 377 | case md_can_flame::REQUEST_RECEIVE_POSITION: //dont useing |
YutaTogashi | 4:6e88615c830b | 378 | //returnMessage.i = encoder.getPosition(); |
YutaTogashi | 4:6e88615c830b | 379 | returnMessage.i = qei.GetPosition(); |
YutaTogashi | 0:4fe08f484460 | 380 | sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_POSITION; |
YutaTogashi | 0:4fe08f484460 | 381 | for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i]; |
YutaTogashi | 0:4fe08f484460 | 382 | can.write(sendMsg); |
YutaTogashi | 0:4fe08f484460 | 383 | break; |
YutaTogashi | 0:4fe08f484460 | 384 | case md_can_flame::REQUEST_RECEIVE_ROTATION: |
YutaTogashi | 4:6e88615c830b | 385 | //returnMessage.f = static_cast<float>(encoder.getRotation()); |
YutaTogashi | 4:6e88615c830b | 386 | returnMessage.f = rotation[0]; |
YutaTogashi | 0:4fe08f484460 | 387 | sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ROTATION; |
YutaTogashi | 0:4fe08f484460 | 388 | for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i]; |
YutaTogashi | 0:4fe08f484460 | 389 | can.write(sendMsg); |
YutaTogashi | 0:4fe08f484460 | 390 | break; |
YutaTogashi | 0:4fe08f484460 | 391 | case md_can_flame::REQUEST_RECEIVE_RPS: |
YutaTogashi | 4:6e88615c830b | 392 | //returnMessage.f = encoder.getRPS(); |
YutaTogashi | 4:6e88615c830b | 393 | returnMessage.f = rps; |
YutaTogashi | 0:4fe08f484460 | 394 | sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPS; |
YutaTogashi | 0:4fe08f484460 | 395 | for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i]; |
YutaTogashi | 0:4fe08f484460 | 396 | can.write(sendMsg); |
YutaTogashi | 0:4fe08f484460 | 397 | break; |
YutaTogashi | 0:4fe08f484460 | 398 | case md_can_flame::REQUEST_RECEIVE_RPM: |
YutaTogashi | 4:6e88615c830b | 399 | //returnMessage.f = encoder.getRPM(); |
YutaTogashi | 4:6e88615c830b | 400 | returnMessage.f = rpm; |
YutaTogashi | 0:4fe08f484460 | 401 | sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPM; |
YutaTogashi | 0:4fe08f484460 | 402 | for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i]; |
YutaTogashi | 0:4fe08f484460 | 403 | can.write(sendMsg); |
YutaTogashi | 0:4fe08f484460 | 404 | break; |
YutaTogashi | 0:4fe08f484460 | 405 | case md_can_flame::REQUEST_PRINTF_DEBAG: |
YutaTogashi | 0:4fe08f484460 | 406 | printfMode = false; |
YutaTogashi | 0:4fe08f484460 | 407 | break; |
YutaTogashi | 0:4fe08f484460 | 408 | case md_can_flame::REQUEST_PRINTF_FORMAT_CSV: |
YutaTogashi | 0:4fe08f484460 | 409 | printfMode = true; |
YutaTogashi | 0:4fe08f484460 | 410 | break; |
YutaTogashi | 0:4fe08f484460 | 411 | case md_can_flame::REQUEST_RESET_QEI: |
YutaTogashi | 4:6e88615c830b | 412 | //encoder.reset(); |
YutaTogashi | 4:6e88615c830b | 413 | qei.Reset(QEI_RESET_POS); |
YutaTogashi | 0:4fe08f484460 | 414 | break; |
YutaTogashi | 0:4fe08f484460 | 415 | case md_can_flame::REQUEST_RESET_MCU: |
YutaTogashi | 0:4fe08f484460 | 416 | NVIC_SystemReset(); |
YutaTogashi | 0:4fe08f484460 | 417 | break; |
YutaTogashi | 0:4fe08f484460 | 418 | default: |
YutaTogashi | 0:4fe08f484460 | 419 | break; |
YutaTogashi | 0:4fe08f484460 | 420 | } |
YutaTogashi | 0:4fe08f484460 | 421 | } else if(ALL_MCU == receiveMessage.id) { |
YutaTogashi | 4:6e88615c830b | 422 | NVIC_SystemReset(); |
YutaTogashi | 0:4fe08f484460 | 423 | } else { |
YutaTogashi | 0:4fe08f484460 | 424 | |
YutaTogashi | 0:4fe08f484460 | 425 | } |
YutaTogashi | 8:f6851c48f94b | 426 | */ |
YutaTogashi | 0:4fe08f484460 | 427 | } |