Hoshino Taiga / Mbed 2 deprecated Tourobo2022_TBCMotorDriver

#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, 22 months ago (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?

UserRevisionLine numberNew 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入力用のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが４つ)
YutaTogashi 5:58c684e2ee47 6 //Ver5 2019Bチーム自動機で使っていたMD(CAN通信のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが３つ)
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(&parameterStatus,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 }