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@6:a3efeb48498a, 2021-10-17 (annotated)

Committer:
YutaTogashi
Date:
Sun Oct 17 07:41:21 2021 +0000
Revision:
6:a3efeb48498a
Parent:
5:58c684e2ee47
Child:
7:b91354ec59bd
20211017 fix PrintfMode REQUEST_PRINTF_FORMAT_CSV

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
YutaTogashi 3:097710997f6b 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 0:4fe08f484460 24 #include "bitCommunication.h"
YutaTogashi 0:4fe08f484460 25 #include "buzzer.h"
YutaTogashi 0:4fe08f484460 26 #include "eeprom.h"
YutaTogashi 0:4fe08f484460 27 #include "eeprom_initalize.h"
YutaTogashi 4:6e88615c830b 28 //#include "QEI.h"
YutaTogashi 4:6e88615c830b 29 #include "qeihw.h"
YutaTogashi 0:4fe08f484460 30 #include "Pid.h"
YutaTogashi 0:4fe08f484460 31 #include "motorDrive_SMB.h"
YutaTogashi 0:4fe08f484460 32 #include "motorDrive_LAP.h"
YutaTogashi 0:4fe08f484460 33 #include "MedianFilter.h"
YutaTogashi 0:4fe08f484460 34 #include "definition.h"
YutaTogashi 0:4fe08f484460 35 /******************************
YutaTogashi 0:4fe08f484460 36 definition instance
YutaTogashi 0:4fe08f484460 37 ******************************/
YutaTogashi 0:4fe08f484460 38 CAN can(CAN_PIN[CAN_RD],CAN_PIN[CAN_TD]);
YutaTogashi 0:4fe08f484460 39 Serial pc(SERIAL_PIN[SERIAL_TX],SERIAL_PIN[SERIAL_RX],communication_baud::PC);
YutaTogashi 0:4fe08f484460 40 BusIn idSwitch(ID_PIN[SWITCH1],ID_PIN[SWITCH2],ID_PIN[SWITCH3],ID_PIN[SWITCH4]);
YutaTogashi 4:6e88615c830b 41 //QEI encoder(ENCODER_PIN[ENCODER_A],ENCODER_PIN[ENCODER_B],NC,ENCODER_PPR);
YutaTogashi 4:6e88615c830b 42 QEIHW qei(QEI_DIRINV_NONE,QEI_SIGNALMODE_QUAD,QEI_CAPMODE_4X,QEI_INVINX_NONE); //QEI_hw
YutaTogashi 0:4fe08f484460 43 buzzer led(LED_PIN[FREE]);
YutaTogashi 0:4fe08f484460 44 PwmOut pwmLed[2] = {
YutaTogashi 0:4fe08f484460 45 PwmOut(LED_PIN[CW]),
YutaTogashi 0:4fe08f484460 46 PwmOut(LED_PIN[CCW]),
YutaTogashi 0:4fe08f484460 47 };
YutaTogashi 0:4fe08f484460 48 Pid pid;
YutaTogashi 0:4fe08f484460 49 Ticker controlCycle;
YutaTogashi 0:4fe08f484460 50 Timer errorTimer;
YutaTogashi 4:6e88615c830b 51 #ifdef MOTOR_DRIVER_MODE_LAP
YutaTogashi 2:40528aee5ebe 52 motorDriveLAP motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
YutaTogashi 2:40528aee5ebe 53 #else
YutaTogashi 2:40528aee5ebe 54 motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
YutaTogashi 2:40528aee5ebe 55 #endif
YutaTogashi 2:40528aee5ebe 56 #if MOTOR_DRIVER_VERSION == 4
YutaTogashi 2:40528aee5ebe 57 DigitalOut pwml(PWML_PIN,true);
YutaTogashi 2:40528aee5ebe 58 #endif
YutaTogashi 0:4fe08f484460 59 /******************************
YutaTogashi 0:4fe08f484460 60 global variable
YutaTogashi 0:4fe08f484460 61 ******************************/
YutaTogashi 0:4fe08f484460 62 float parameter[PARAMETER_NUMBER] = {0.0f,0.0f,0.0f,0.0f};
YutaTogashi 0:4fe08f484460 63 float outputDuty = 0.0f;
YutaTogashi 0:4fe08f484460 64 float targetRpm = 0.0f;
YutaTogashi 4:6e88615c830b 65 float rps = 0.0f;
YutaTogashi 4:6e88615c830b 66 float rpm = 0.0f;
YutaTogashi 4:6e88615c830b 67 float rotation[2] = {0.0f,0.0f};
YutaTogashi 0:4fe08f484460 68 int powerFilterCount;
YutaTogashi 0:4fe08f484460 69 int powerFilterArray[10];
YutaTogashi 3:097710997f6b 70 bool enable_motor_output = false; //モーターへの出力許可を管理(true : 出力可能 , false : 出力を強制停止)
YutaTogashi 0:4fe08f484460 71 bool outputMode = false;
YutaTogashi 0:4fe08f484460 72 bool printfMode = false;
YutaTogashi 0:4fe08f484460 73 bool receiveParameterStatus[4] = {false,false,false,false};
YutaTogashi 0:4fe08f484460 74 /******************************
YutaTogashi 0:4fe08f484460 75 definition function
YutaTogashi 0:4fe08f484460 76 ******************************/
YutaTogashi 0:4fe08f484460 77 void initalize();
YutaTogashi 0:4fe08f484460 78 void mainLoop();
YutaTogashi 3:097710997f6b 79 void outputIndicator(float control_duty);
YutaTogashi 3:097710997f6b 80 void safetyControl(float duty);
YutaTogashi 0:4fe08f484460 81 void canReceiveHandler();
YutaTogashi 0:4fe08f484460 82 void debag();
YutaTogashi 0:4fe08f484460 83 /******************************
YutaTogashi 0:4fe08f484460 84 function content
YutaTogashi 0:4fe08f484460 85 ******************************/
YutaTogashi 0:4fe08f484460 86 int main() {
YutaTogashi 0:4fe08f484460 87 initalize();
YutaTogashi 0:4fe08f484460 88 while(1){
YutaTogashi 0:4fe08f484460 89 }
YutaTogashi 0:4fe08f484460 90 }
YutaTogashi 0:4fe08f484460 91
YutaTogashi 0:4fe08f484460 92 void initalize() {
YutaTogashi 4:6e88615c830b 93 /***************************EEPROM　読み取り処理*******************************/
YutaTogashi 0:4fe08f484460 94 can_flame eepromData;
YutaTogashi 0:4fe08f484460 95 Chip_EEPROM_init();
YutaTogashi 0:4fe08f484460 96 __disable_irq();
YutaTogashi 0:4fe08f484460 97 read_eeprom((char*)0, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 98 parameter[KP] = eepromData.f;
YutaTogashi 0:4fe08f484460 99 read_eeprom((char*)4, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 100 parameter[KI] = eepromData.f;
YutaTogashi 0:4fe08f484460 101 read_eeprom((char*)8, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 102 parameter[KD] = eepromData.f;
YutaTogashi 0:4fe08f484460 103 __enable_irq();
YutaTogashi 4:6e88615c830b 104 /***************************************************************************/
YutaTogashi 4:6e88615c830b 105
YutaTogashi 4:6e88615c830b 106 /******************************割り込み優先度設定******************************/
YutaTogashi 2:40528aee5ebe 107 NVIC_SetPriority(C_CAN0_IRQn,0);
YutaTogashi 2:40528aee5ebe 108 NVIC_SetPriority(QEI_IRQn,1);
YutaTogashi 4:6e88615c830b 109 /***************************************************************************/
YutaTogashi 0:4fe08f484460 110
YutaTogashi 4:6e88615c830b 111 /*******************************CAN通信の設定*********************************/
YutaTogashi 0:4fe08f484460 112 can.frequency(communication_baud::CAN);
YutaTogashi 2:40528aee5ebe 113 can.attach(&canReceiveHandler,CAN::RxIrq);
YutaTogashi 4:6e88615c830b 114 /***************************************************************************/
YutaTogashi 2:40528aee5ebe 115
YutaTogashi 4:6e88615c830b 116 /***************************MotorDrive 設定**********************************/
YutaTogashi 0:4fe08f484460 117 motor.setupFrequency(motor_option::FREQUENCY);
YutaTogashi 0:4fe08f484460 118 motor.setupLimitDuty(motor_option::LOWER_LIMIT,motor_option::UPPER_LIMIT);
YutaTogashi 4:6e88615c830b 119 /****************************************************************************/
YutaTogashi 4:6e88615c830b 120
YutaTogashi 4:6e88615c830b 121 /*********************************SpeedPID設定*******************************/
YutaTogashi 0:4fe08f484460 122 pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
YutaTogashi 4:6e88615c830b 123 /***************************************************************************/
YutaTogashi 0:4fe08f484460 124
YutaTogashi 4:6e88615c830b 125 /**********************************その他************************************/
YutaTogashi 4:6e88615c830b 126 qei.SetDigiFilter(480UL); //QEI_hw filter設定
YutaTogashi 4:6e88615c830b 127 qei.SetMaxPosition(0xFFFFFFFF); //QEI_hw filter設定
YutaTogashi 4:6e88615c830b 128 qei.SetVelocityTimerReload_us(10000); //QEI_hw filter設定
YutaTogashi 4:6e88615c830b 129
YutaTogashi 4:6e88615c830b 130 errorTimer.start(); //CAN通信 タイムアウト検知用計測開始
YutaTogashi 5:58c684e2ee47 131 pc.printf("\r\nInitalize MotorDriver!\r\n"); //initalize終了出力
YutaTogashi 5:58c684e2ee47 132 pc.printf("Firmware:robocon2021_TBCMotorDriver( https://os.mbed.com/users/YutaTogashi/code/robocon2021_TBCMotorDriver/ )\r\n"); //Firmwareをシリアル出力
YutaTogashi 5:58c684e2ee47 133 pc.printf("UsingLibrary:can_network_motordriver ( https://os.mbed.com/users/YutaTogashi/code/can_network_motordriver/ )\r\n"); //MDを使用しやすくしたライブラリ　シリアル出力
YutaTogashi 4:6e88615c830b 134 controlCycle.attach(&mainLoop,period::CONTROL_CYCLE); //制御ループ設定
YutaTogashi 4:6e88615c830b 135 /***************************************************************************/
YutaTogashi 0:4fe08f484460 136 }
YutaTogashi 0:4fe08f484460 137
YutaTogashi 2:40528aee5ebe 138 void mainLoop() {
YutaTogashi 4:6e88615c830b 139 //RPS,RPMの計算
YutaTogashi 4:6e88615c830b 140 rotation[0] = static_cast<float>(static_cast<int32_t>(qei.GetPosition())) / (static_cast<float>(ENCODER_PPR) * 4.0f);
YutaTogashi 4:6e88615c830b 141 rps = (rotation[0] - rotation[1]) / period::CONTROL_CYCLE;
YutaTogashi 4:6e88615c830b 142 rpm = rps * 60.0f;
YutaTogashi 4:6e88615c830b 143 rotation[1] = rotation[0];
YutaTogashi 4:6e88615c830b 144
YutaTogashi 4:6e88615c830b 145 //速度PIDモードの挙動
YutaTogashi 0:4fe08f484460 146 if(outputMode) {
YutaTogashi 0:4fe08f484460 147 pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
YutaTogashi 4:6e88615c830b 148 pid.calculate(targetRpm,/*encoder.getRPM()*/rpm);
YutaTogashi 0:4fe08f484460 149 outputDuty = pid.getDuty();
YutaTogashi 0:4fe08f484460 150 }
YutaTogashi 0:4fe08f484460 151
YutaTogashi 4:6e88615c830b 152 //TIMEOUT判定
YutaTogashi 3:097710997f6b 153 if(errorTimer.read() > TIMEOUT_VALUE) enable_motor_output = false;
YutaTogashi 3:097710997f6b 154 else enable_motor_output = true;
YutaTogashi 3:097710997f6b 155
YutaTogashi 4:6e88615c830b 156 //出力処理
YutaTogashi 3:097710997f6b 157 if(enable_motor_output) {
YutaTogashi 4:6e88615c830b 158 led.output(outputMode); //速度PIDモードの場合は，LEDインジゲータが点灯
YutaTogashi 3:097710997f6b 159 } else {
YutaTogashi 4:6e88615c830b 160 led.output(0.5f); //TIMEOUT時は，LEDインジゲータが点滅
YutaTogashi 4:6e88615c830b 161 safetyControl(0.05f); //徐々に出力を下げる
YutaTogashi 4:6e88615c830b 162 pid.reset(); //速度PIDの計算をリセット
YutaTogashi 3:097710997f6b 163 }
YutaTogashi 2:40528aee5ebe 164
YutaTogashi 4:6e88615c830b 165 outputIndicator(outputDuty); //Duty,Phaseインジゲータを制御
YutaTogashi 4:6e88615c830b 166 motor.output(outputDuty); //A3921制御
YutaTogashi 4:6e88615c830b 167
YutaTogashi 4:6e88615c830b 168 debag(); //デバッグ関数
YutaTogashi 0:4fe08f484460 169 }
YutaTogashi 0:4fe08f484460 170
YutaTogashi 0:4fe08f484460 171 void debag() {
YutaTogashi 0:4fe08f484460 172 if(!(printfMode)) {
YutaTogashi 0:4fe08f484460 173 pc.printf("SWITCH:%d\t",idSwitch.read());
YutaTogashi 4:6e88615c830b 174 pc.printf("CAN_ID:%d\t", (MD_0_MCU + idSwitch.read()));
YutaTogashi 0:4fe08f484460 175 pc.printf("DUTY:%.2f\t",outputDuty);
YutaTogashi 4:6e88615c830b 176 pc.printf("RPM:%.2f\t",/*encoder.getRPM()*/rpm);
YutaTogashi 0:4fe08f484460 177
YutaTogashi 0:4fe08f484460 178 if(outputMode) {
YutaTogashi 0:4fe08f484460 179 pc.printf("TARGET:%.1f\t",targetRpm);
YutaTogashi 0:4fe08f484460 180 pc.printf("Kp:%f\t",parameter[KP]);
YutaTogashi 0:4fe08f484460 181 pc.printf("Ki:%f\t",parameter[KI]);
YutaTogashi 0:4fe08f484460 182 pc.printf("Kd:%f\t",parameter[KD]);
YutaTogashi 0:4fe08f484460 183 }
YutaTogashi 0:4fe08f484460 184 pc.printf("TIME:%f",errorTimer.read());
YutaTogashi 0:4fe08f484460 185 pc.printf("\r\n");
YutaTogashi 0:4fe08f484460 186 } else {
YutaTogashi 6:a3efeb48498a 187 pc.printf("%f,",targetRpm);
YutaTogashi 4:6e88615c830b 188 pc.printf("%f,",/*encoder.getRPM()*/rpm);
YutaTogashi 6:a3efeb48498a 189 pc.printf("%f",outputDuty);
YutaTogashi 0:4fe08f484460 190 pc.printf("\r\n");
YutaTogashi 0:4fe08f484460 191 }
YutaTogashi 0:4fe08f484460 192 }
YutaTogashi 0:4fe08f484460 193
YutaTogashi 2:40528aee5ebe 194 void outputIndicator(float duty) {
YutaTogashi 2:40528aee5ebe 195 if(fabs(duty) > 0.0f) {
YutaTogashi 2:40528aee5ebe 196 if(duty > 0.0f) {
YutaTogashi 2:40528aee5ebe 197 pwmLed[CW].write(fabs(duty));
YutaTogashi 0:4fe08f484460 198 pwmLed[CCW].write(0.0f);
YutaTogashi 0:4fe08f484460 199 } else {
YutaTogashi 0:4fe08f484460 200 pwmLed[CW].write(0.0f);
YutaTogashi 2:40528aee5ebe 201 pwmLed[CCW].write(fabs(duty));
YutaTogashi 0:4fe08f484460 202 }
YutaTogashi 0:4fe08f484460 203 } else {
YutaTogashi 0:4fe08f484460 204 pwmLed[CW].write(0.0f);
YutaTogashi 0:4fe08f484460 205 pwmLed[CCW].write(0.0f);
YutaTogashi 0:4fe08f484460 206 }
YutaTogashi 0:4fe08f484460 207 }
YutaTogashi 0:4fe08f484460 208
YutaTogashi 3:097710997f6b 209 void safetyControl(float control_duty) {
YutaTogashi 4:6e88615c830b 210 if(fabs(outputDuty - 0.0f) > 0.10f) {
YutaTogashi 3:097710997f6b 211 if(outputDuty > 0.0f) outputDuty -= control_duty;
YutaTogashi 3:097710997f6b 212 else outputDuty += control_duty;
YutaTogashi 3:097710997f6b 213 } else {
YutaTogashi 3:097710997f6b 214 outputDuty = 0.0f;
YutaTogashi 3:097710997f6b 215 }
YutaTogashi 3:097710997f6b 216 }
YutaTogashi 3:097710997f6b 217
YutaTogashi 0:4fe08f484460 218 void canReceiveHandler() {
YutaTogashi 0:4fe08f484460 219 can_flame message,returnMessage;
YutaTogashi 0:4fe08f484460 220 CANMessage sendMsg,receiveMessage;
YutaTogashi 0:4fe08f484460 221
YutaTogashi 0:4fe08f484460 222 can.read(receiveMessage);
YutaTogashi 0:4fe08f484460 223
YutaTogashi 0:4fe08f484460 224 sendMsg.id = MAIN_MCU;
YutaTogashi 0:4fe08f484460 225
YutaTogashi 0:4fe08f484460 226 if((MD_0_MCU + idSwitch.read()) == receiveMessage.id) {
YutaTogashi 0:4fe08f484460 227 errorTimer.reset();
YutaTogashi 0:4fe08f484460 228 switch ((int)receiveMessage.data[0]) {
YutaTogashi 0:4fe08f484460 229 case md_can_flame::SEND_KP:
YutaTogashi 0:4fe08f484460 230 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 231 __disable_irq();
YutaTogashi 0:4fe08f484460 232 write_eeprom(message.c,(char*)0,4);
YutaTogashi 0:4fe08f484460 233 __enable_irq();
YutaTogashi 0:4fe08f484460 234 parameter[KP] = message.f;
YutaTogashi 0:4fe08f484460 235 receiveParameterStatus[0] = true;
YutaTogashi 0:4fe08f484460 236 break;
YutaTogashi 0:4fe08f484460 237 case md_can_flame::SEND_KI:
YutaTogashi 0:4fe08f484460 238 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 239 __disable_irq();
YutaTogashi 0:4fe08f484460 240 write_eeprom(message.c,(char*)4,4);
YutaTogashi 0:4fe08f484460 241 __enable_irq();
YutaTogashi 0:4fe08f484460 242 parameter[KI] = message.f;
YutaTogashi 0:4fe08f484460 243 receiveParameterStatus[1] = true;
YutaTogashi 0:4fe08f484460 244 break;
YutaTogashi 0:4fe08f484460 245 case md_can_flame::SEND_KD:
YutaTogashi 0:4fe08f484460 246 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 247 __disable_irq();
YutaTogashi 0:4fe08f484460 248 write_eeprom(message.c,(char*)8,4);
YutaTogashi 0:4fe08f484460 249 __enable_irq();
YutaTogashi 0:4fe08f484460 250 parameter[KD] = message.f;
YutaTogashi 0:4fe08f484460 251 receiveParameterStatus[2] = true;
YutaTogashi 0:4fe08f484460 252 break;
YutaTogashi 0:4fe08f484460 253 case md_can_flame::SEND_ACCELERATION:
YutaTogashi 0:4fe08f484460 254 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 255 parameter[ACCELERATION] = message.f;
YutaTogashi 0:4fe08f484460 256 receiveParameterStatus[3] = true;
YutaTogashi 0:4fe08f484460 257 break;
YutaTogashi 0:4fe08f484460 258 case md_can_flame::SEND_CONTROL_DUTY:
YutaTogashi 0:4fe08f484460 259 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 260 outputDuty = message.f;
YutaTogashi 0:4fe08f484460 261 outputMode = false;
YutaTogashi 0:4fe08f484460 262 break;
YutaTogashi 0:4fe08f484460 263 case md_can_flame::SEND_CONTROL_RPM:
YutaTogashi 0:4fe08f484460 264 if(bitRead((int)receiveMessage.data[1],0)) {
YutaTogashi 0:4fe08f484460 265 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 266 targetRpm = message.f;
YutaTogashi 0:4fe08f484460 267 outputMode = true;
YutaTogashi 0:4fe08f484460 268 } else {
YutaTogashi 0:4fe08f484460 269 targetRpm = 0.0f;
YutaTogashi 0:4fe08f484460 270 outputDuty = 0.0f;
YutaTogashi 0:4fe08f484460 271 pid.reset();
YutaTogashi 0:4fe08f484460 272 outputMode = false;
YutaTogashi 0:4fe08f484460 273 }
YutaTogashi 0:4fe08f484460 274 break;
YutaTogashi 0:4fe08f484460 275 case md_can_flame::REQUEST_RECEIVE_CURRENT:
YutaTogashi 0:4fe08f484460 276 //no code
YutaTogashi 0:4fe08f484460 277 break;
YutaTogashi 0:4fe08f484460 278 case md_can_flame::REQUEST_RECEIVE_ERROR:
YutaTogashi 0:4fe08f484460 279 int parameterStatus = 0;
YutaTogashi 0:4fe08f484460 280 for(int i=0;i<4;i++) bitWrite(&parameterStatus,i,receiveParameterStatus[i]);
YutaTogashi 0:4fe08f484460 281 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ERROR;
YutaTogashi 0:4fe08f484460 282 sendMsg.data[2] = parameterStatus;
YutaTogashi 0:4fe08f484460 283 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 284 break;
YutaTogashi 4:6e88615c830b 285 case md_can_flame::REQUEST_RECEIVE_POSITION: //dont useing
YutaTogashi 4:6e88615c830b 286 //returnMessage.i = encoder.getPosition();
YutaTogashi 4:6e88615c830b 287 returnMessage.i = qei.GetPosition();
YutaTogashi 0:4fe08f484460 288 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_POSITION;
YutaTogashi 0:4fe08f484460 289 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 290 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 291 break;
YutaTogashi 0:4fe08f484460 292 case md_can_flame::REQUEST_RECEIVE_ROTATION:
YutaTogashi 4:6e88615c830b 293 //returnMessage.f = static_cast<float>(encoder.getRotation());
YutaTogashi 4:6e88615c830b 294 returnMessage.f = rotation[0];
YutaTogashi 0:4fe08f484460 295 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ROTATION;
YutaTogashi 0:4fe08f484460 296 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 297 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 298 break;
YutaTogashi 0:4fe08f484460 299 case md_can_flame::REQUEST_RECEIVE_RPS:
YutaTogashi 4:6e88615c830b 300 //returnMessage.f = encoder.getRPS();
YutaTogashi 4:6e88615c830b 301 returnMessage.f = rps;
YutaTogashi 0:4fe08f484460 302 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPS;
YutaTogashi 0:4fe08f484460 303 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 304 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 305 break;
YutaTogashi 0:4fe08f484460 306 case md_can_flame::REQUEST_RECEIVE_RPM:
YutaTogashi 4:6e88615c830b 307 //returnMessage.f = encoder.getRPM();
YutaTogashi 4:6e88615c830b 308 returnMessage.f = rpm;
YutaTogashi 0:4fe08f484460 309 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPM;
YutaTogashi 0:4fe08f484460 310 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 311 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 312 break;
YutaTogashi 0:4fe08f484460 313 case md_can_flame::REQUEST_PRINTF_DEBAG:
YutaTogashi 0:4fe08f484460 314 printfMode = false;
YutaTogashi 0:4fe08f484460 315 break;
YutaTogashi 0:4fe08f484460 316 case md_can_flame::REQUEST_PRINTF_FORMAT_CSV:
YutaTogashi 0:4fe08f484460 317 printfMode = true;
YutaTogashi 0:4fe08f484460 318 break;
YutaTogashi 0:4fe08f484460 319 case md_can_flame::REQUEST_RESET_QEI:
YutaTogashi 4:6e88615c830b 320 //encoder.reset();
YutaTogashi 4:6e88615c830b 321 qei.Reset(QEI_RESET_POS);
YutaTogashi 0:4fe08f484460 322 break;
YutaTogashi 0:4fe08f484460 323 case md_can_flame::REQUEST_RESET_MCU:
YutaTogashi 0:4fe08f484460 324 NVIC_SystemReset();
YutaTogashi 0:4fe08f484460 325 break;
YutaTogashi 0:4fe08f484460 326 default:
YutaTogashi 0:4fe08f484460 327 break;
YutaTogashi 0:4fe08f484460 328 }
YutaTogashi 0:4fe08f484460 329 } else if(ALL_MCU == receiveMessage.id) {
YutaTogashi 4:6e88615c830b 330 NVIC_SystemReset();
YutaTogashi 0:4fe08f484460 331 } else {
YutaTogashi 0:4fe08f484460 332
YutaTogashi 0:4fe08f484460 333 }
YutaTogashi 0:4fe08f484460 334 }