#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

Committer:
YutaTogashi
Date:
Wed Apr 08 07:50:27 2020 +0000
Revision:
2:40528aee5ebe
Parent:
0:4fe08f484460
Child:
3:097710997f6b
20200408 Add LAPcontrol & Ver4,5,6 Control

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 2:40528aee5ebe 4 //<注意>プログラム上でのバージョンです
YutaTogashi 2:40528aee5ebe 5 //Ver4 2019Bチーム手動機で使っていたMD
YutaTogashi 2:40528aee5ebe 6 //Ver5 2019Bチーム自動機で使っていたMD
YutaTogashi 2:40528aee5ebe 7 //Ver6 センサ入力に対応したMD
YutaTogashi 2:40528aee5ebe 8 #define MOTOR_DRIVER_VERSION 6
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 0:4fe08f484460 23 #include "can_network_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 0:4fe08f484460 28 #include "QEI.h"
YutaTogashi 0:4fe08f484460 29 #include "Pid.h"
YutaTogashi 0:4fe08f484460 30 #include "motorDrive_SMB.h"
YutaTogashi 0:4fe08f484460 31 #include "motorDrive_LAP.h"
YutaTogashi 0:4fe08f484460 32 #include "MedianFilter.h"
YutaTogashi 0:4fe08f484460 33 #include "definition.h"
YutaTogashi 0:4fe08f484460 34 /******************************
YutaTogashi 0:4fe08f484460 35 definition instance
YutaTogashi 0:4fe08f484460 36 ******************************/
YutaTogashi 0:4fe08f484460 37 CAN can(CAN_PIN[CAN_RD],CAN_PIN[CAN_TD]);
YutaTogashi 0:4fe08f484460 38 Serial pc(SERIAL_PIN[SERIAL_TX],SERIAL_PIN[SERIAL_RX],communication_baud::PC);
YutaTogashi 0:4fe08f484460 39 BusIn idSwitch(ID_PIN[SWITCH1],ID_PIN[SWITCH2],ID_PIN[SWITCH3],ID_PIN[SWITCH4]);
YutaTogashi 0:4fe08f484460 40 QEI encoder(ENCODER_PIN[ENCODER_A],ENCODER_PIN[ENCODER_B],NC,ENCODER_PPR);
YutaTogashi 2:40528aee5ebe 41 //motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
YutaTogashi 0:4fe08f484460 42 buzzer led(LED_PIN[FREE]);
YutaTogashi 0:4fe08f484460 43 PwmOut pwmLed[2] = {
YutaTogashi 0:4fe08f484460 44 PwmOut(LED_PIN[CW]),
YutaTogashi 0:4fe08f484460 45 PwmOut(LED_PIN[CCW]),
YutaTogashi 0:4fe08f484460 46 };
YutaTogashi 0:4fe08f484460 47 Pid pid;
YutaTogashi 0:4fe08f484460 48 Ticker controlCycle;
YutaTogashi 0:4fe08f484460 49 Timer errorTimer;
YutaTogashi 2:40528aee5ebe 50 #ifndef MOTOR_DRIVER_MODE_LAP
YutaTogashi 2:40528aee5ebe 51 motorDriveLAP motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
YutaTogashi 2:40528aee5ebe 52 #else
YutaTogashi 2:40528aee5ebe 53 motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
YutaTogashi 2:40528aee5ebe 54 #endif
YutaTogashi 2:40528aee5ebe 55 #if MOTOR_DRIVER_VERSION == 4
YutaTogashi 2:40528aee5ebe 56 DigitalOut pwml(PWML_PIN,true);
YutaTogashi 2:40528aee5ebe 57 #endif
YutaTogashi 0:4fe08f484460 58 /******************************
YutaTogashi 0:4fe08f484460 59 global variable
YutaTogashi 0:4fe08f484460 60 ******************************/
YutaTogashi 0:4fe08f484460 61 float parameter[PARAMETER_NUMBER] = {0.0f,0.0f,0.0f,0.0f};
YutaTogashi 0:4fe08f484460 62 float outputDuty = 0.0f;
YutaTogashi 0:4fe08f484460 63 float targetRpm = 0.0f;
YutaTogashi 0:4fe08f484460 64 int powerFilterCount;
YutaTogashi 0:4fe08f484460 65 int powerFilterArray[10];
YutaTogashi 0:4fe08f484460 66 bool outputMode = false;
YutaTogashi 0:4fe08f484460 67 bool printfMode = false;
YutaTogashi 0:4fe08f484460 68 bool receiveParameterStatus[4] = {false,false,false,false};
YutaTogashi 0:4fe08f484460 69 /******************************
YutaTogashi 0:4fe08f484460 70 definition function
YutaTogashi 0:4fe08f484460 71 ******************************/
YutaTogashi 0:4fe08f484460 72 void initalize();
YutaTogashi 0:4fe08f484460 73 void mainLoop();
YutaTogashi 2:40528aee5ebe 74 void outputIndicator(float duty);
YutaTogashi 0:4fe08f484460 75 void canReceiveHandler();
YutaTogashi 0:4fe08f484460 76 void debag();
YutaTogashi 0:4fe08f484460 77 /******************************
YutaTogashi 0:4fe08f484460 78 function content
YutaTogashi 0:4fe08f484460 79 ******************************/
YutaTogashi 0:4fe08f484460 80 int main() {
YutaTogashi 0:4fe08f484460 81 initalize();
YutaTogashi 0:4fe08f484460 82 while(1){
YutaTogashi 0:4fe08f484460 83 }
YutaTogashi 0:4fe08f484460 84 }
YutaTogashi 0:4fe08f484460 85
YutaTogashi 0:4fe08f484460 86 void initalize() {
YutaTogashi 0:4fe08f484460 87 can_flame eepromData;
YutaTogashi 0:4fe08f484460 88
YutaTogashi 0:4fe08f484460 89 Chip_EEPROM_init();
YutaTogashi 0:4fe08f484460 90 __disable_irq();
YutaTogashi 0:4fe08f484460 91 read_eeprom((char*)0, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 92 parameter[KP] = eepromData.f;
YutaTogashi 0:4fe08f484460 93 read_eeprom((char*)4, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 94 parameter[KI] = eepromData.f;
YutaTogashi 0:4fe08f484460 95 read_eeprom((char*)8, eepromData.c, 4);
YutaTogashi 0:4fe08f484460 96 parameter[KD] = eepromData.f;
YutaTogashi 0:4fe08f484460 97 __enable_irq();
YutaTogashi 2:40528aee5ebe 98
YutaTogashi 2:40528aee5ebe 99 NVIC_SetPriority(C_CAN0_IRQn,0);
YutaTogashi 2:40528aee5ebe 100 NVIC_SetPriority(QEI_IRQn,1);
YutaTogashi 0:4fe08f484460 101
YutaTogashi 0:4fe08f484460 102 can.frequency(communication_baud::CAN);
YutaTogashi 2:40528aee5ebe 103 can.attach(&canReceiveHandler,CAN::RxIrq);
YutaTogashi 2:40528aee5ebe 104
YutaTogashi 2:40528aee5ebe 105 wait_ms(1000);
YutaTogashi 0:4fe08f484460 106
YutaTogashi 0:4fe08f484460 107 motor.setupFrequency(motor_option::FREQUENCY);
YutaTogashi 0:4fe08f484460 108 motor.setupLimitDuty(motor_option::LOWER_LIMIT,motor_option::UPPER_LIMIT);
YutaTogashi 2:40528aee5ebe 109
YutaTogashi 0:4fe08f484460 110 pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
YutaTogashi 0:4fe08f484460 111
YutaTogashi 0:4fe08f484460 112 errorTimer.start();
YutaTogashi 0:4fe08f484460 113 pc.printf("Initalize MotorDriver!\r\n\r\n");
YutaTogashi 0:4fe08f484460 114 controlCycle.attach(&mainLoop,period::CONTROL_CYCLE);
YutaTogashi 0:4fe08f484460 115 }
YutaTogashi 0:4fe08f484460 116
YutaTogashi 2:40528aee5ebe 117 void mainLoop() {
YutaTogashi 0:4fe08f484460 118 if(outputMode) {
YutaTogashi 0:4fe08f484460 119 pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
YutaTogashi 0:4fe08f484460 120 pid.calculate(targetRpm,encoder.getRPM());
YutaTogashi 0:4fe08f484460 121 outputDuty = pid.getDuty();
YutaTogashi 0:4fe08f484460 122 }
YutaTogashi 0:4fe08f484460 123
YutaTogashi 0:4fe08f484460 124 if(errorTimer.read() > TIMEOUT_VALUE) {
YutaTogashi 0:4fe08f484460 125 led.output(0.5f);
YutaTogashi 0:4fe08f484460 126 outputDuty = 0.0f;
YutaTogashi 0:4fe08f484460 127 } else {
YutaTogashi 0:4fe08f484460 128 led.output(outputMode);
YutaTogashi 0:4fe08f484460 129 }
YutaTogashi 0:4fe08f484460 130
YutaTogashi 2:40528aee5ebe 131 outputIndicator(outputDuty);
YutaTogashi 0:4fe08f484460 132 motor.output(outputDuty);
YutaTogashi 0:4fe08f484460 133
YutaTogashi 0:4fe08f484460 134 debag();
YutaTogashi 2:40528aee5ebe 135
YutaTogashi 0:4fe08f484460 136 }
YutaTogashi 0:4fe08f484460 137
YutaTogashi 0:4fe08f484460 138 void debag() {
YutaTogashi 0:4fe08f484460 139 if(!(printfMode)) {
YutaTogashi 0:4fe08f484460 140 pc.printf("SWITCH:%d\t",idSwitch.read());
YutaTogashi 0:4fe08f484460 141 pc.printf("DUTY:%.2f\t",outputDuty);
YutaTogashi 0:4fe08f484460 142 pc.printf("RPM:%.2f\t",encoder.getRPM());
YutaTogashi 0:4fe08f484460 143
YutaTogashi 0:4fe08f484460 144 if(outputMode) {
YutaTogashi 0:4fe08f484460 145 pc.printf("TARGET:%.1f\t",targetRpm);
YutaTogashi 0:4fe08f484460 146 pc.printf("Kp:%f\t",parameter[KP]);
YutaTogashi 0:4fe08f484460 147 pc.printf("Ki:%f\t",parameter[KI]);
YutaTogashi 0:4fe08f484460 148 pc.printf("Kd:%f\t",parameter[KD]);
YutaTogashi 0:4fe08f484460 149 }
YutaTogashi 0:4fe08f484460 150 pc.printf("TIME:%f",errorTimer.read());
YutaTogashi 0:4fe08f484460 151 pc.printf("\r\n");
YutaTogashi 0:4fe08f484460 152 } else {
YutaTogashi 2:40528aee5ebe 153 pc.printf("%f",targetRpm);
YutaTogashi 2:40528aee5ebe 154 pc.printf("%f,",encoder.getRPM());
YutaTogashi 0:4fe08f484460 155 pc.printf(",%f",outputDuty);
YutaTogashi 0:4fe08f484460 156 pc.printf("\r\n");
YutaTogashi 0:4fe08f484460 157 }
YutaTogashi 0:4fe08f484460 158 }
YutaTogashi 0:4fe08f484460 159
YutaTogashi 2:40528aee5ebe 160 void outputIndicator(float duty) {
YutaTogashi 2:40528aee5ebe 161 if(fabs(duty) > 0.0f) {
YutaTogashi 2:40528aee5ebe 162 if(duty > 0.0f) {
YutaTogashi 2:40528aee5ebe 163 pwmLed[CW].write(fabs(duty));
YutaTogashi 0:4fe08f484460 164 pwmLed[CCW].write(0.0f);
YutaTogashi 0:4fe08f484460 165 } else {
YutaTogashi 0:4fe08f484460 166 pwmLed[CW].write(0.0f);
YutaTogashi 2:40528aee5ebe 167 pwmLed[CCW].write(fabs(duty));
YutaTogashi 0:4fe08f484460 168 }
YutaTogashi 0:4fe08f484460 169 } else {
YutaTogashi 0:4fe08f484460 170 pwmLed[CW].write(0.0f);
YutaTogashi 0:4fe08f484460 171 pwmLed[CCW].write(0.0f);
YutaTogashi 0:4fe08f484460 172 }
YutaTogashi 0:4fe08f484460 173 }
YutaTogashi 0:4fe08f484460 174
YutaTogashi 0:4fe08f484460 175 void canReceiveHandler() {
YutaTogashi 0:4fe08f484460 176 can_flame message,returnMessage;
YutaTogashi 0:4fe08f484460 177 CANMessage sendMsg,receiveMessage;
YutaTogashi 0:4fe08f484460 178
YutaTogashi 0:4fe08f484460 179 can.read(receiveMessage);
YutaTogashi 0:4fe08f484460 180
YutaTogashi 0:4fe08f484460 181 sendMsg.id = MAIN_MCU;
YutaTogashi 0:4fe08f484460 182
YutaTogashi 0:4fe08f484460 183 if((MD_0_MCU + idSwitch.read()) == receiveMessage.id) {
YutaTogashi 0:4fe08f484460 184 errorTimer.reset();
YutaTogashi 0:4fe08f484460 185 switch ((int)receiveMessage.data[0]) {
YutaTogashi 0:4fe08f484460 186 case md_can_flame::SEND_KP:
YutaTogashi 0:4fe08f484460 187 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 188 __disable_irq();
YutaTogashi 0:4fe08f484460 189 write_eeprom(message.c,(char*)0,4);
YutaTogashi 0:4fe08f484460 190 __enable_irq();
YutaTogashi 0:4fe08f484460 191 parameter[KP] = message.f;
YutaTogashi 0:4fe08f484460 192 receiveParameterStatus[0] = true;
YutaTogashi 0:4fe08f484460 193 break;
YutaTogashi 0:4fe08f484460 194 case md_can_flame::SEND_KI:
YutaTogashi 0:4fe08f484460 195 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 196 __disable_irq();
YutaTogashi 0:4fe08f484460 197 write_eeprom(message.c,(char*)4,4);
YutaTogashi 0:4fe08f484460 198 __enable_irq();
YutaTogashi 0:4fe08f484460 199 parameter[KI] = message.f;
YutaTogashi 0:4fe08f484460 200 receiveParameterStatus[1] = true;
YutaTogashi 0:4fe08f484460 201 break;
YutaTogashi 0:4fe08f484460 202 case md_can_flame::SEND_KD:
YutaTogashi 0:4fe08f484460 203 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 204 __disable_irq();
YutaTogashi 0:4fe08f484460 205 write_eeprom(message.c,(char*)8,4);
YutaTogashi 0:4fe08f484460 206 __enable_irq();
YutaTogashi 0:4fe08f484460 207 parameter[KD] = message.f;
YutaTogashi 0:4fe08f484460 208 receiveParameterStatus[2] = true;
YutaTogashi 0:4fe08f484460 209 break;
YutaTogashi 0:4fe08f484460 210 case md_can_flame::SEND_ACCELERATION:
YutaTogashi 0:4fe08f484460 211 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 212 parameter[ACCELERATION] = message.f;
YutaTogashi 0:4fe08f484460 213 receiveParameterStatus[3] = true;
YutaTogashi 0:4fe08f484460 214 break;
YutaTogashi 0:4fe08f484460 215 case md_can_flame::SEND_CONTROL_DUTY:
YutaTogashi 0:4fe08f484460 216 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 217 outputDuty = message.f;
YutaTogashi 0:4fe08f484460 218 outputMode = false;
YutaTogashi 0:4fe08f484460 219 break;
YutaTogashi 0:4fe08f484460 220 case md_can_flame::SEND_CONTROL_RPM:
YutaTogashi 0:4fe08f484460 221 if(bitRead((int)receiveMessage.data[1],0)) {
YutaTogashi 0:4fe08f484460 222 for(int i=2;i<6;i++) message.c[i-2] = receiveMessage.data[i];
YutaTogashi 0:4fe08f484460 223 targetRpm = message.f;
YutaTogashi 0:4fe08f484460 224 outputMode = true;
YutaTogashi 0:4fe08f484460 225 } else {
YutaTogashi 0:4fe08f484460 226 targetRpm = 0.0f;
YutaTogashi 0:4fe08f484460 227 outputDuty = 0.0f;
YutaTogashi 0:4fe08f484460 228 pid.reset();
YutaTogashi 0:4fe08f484460 229 outputMode = false;
YutaTogashi 0:4fe08f484460 230 }
YutaTogashi 0:4fe08f484460 231 break;
YutaTogashi 0:4fe08f484460 232 case md_can_flame::REQUEST_RECEIVE_CURRENT:
YutaTogashi 0:4fe08f484460 233 //no code
YutaTogashi 0:4fe08f484460 234 break;
YutaTogashi 0:4fe08f484460 235 case md_can_flame::REQUEST_RECEIVE_ERROR:
YutaTogashi 0:4fe08f484460 236 int parameterStatus = 0;
YutaTogashi 0:4fe08f484460 237 for(int i=0;i<4;i++) bitWrite(&parameterStatus,i,receiveParameterStatus[i]);
YutaTogashi 0:4fe08f484460 238 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ERROR;
YutaTogashi 0:4fe08f484460 239 sendMsg.data[2] = parameterStatus;
YutaTogashi 0:4fe08f484460 240 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 241 break;
YutaTogashi 0:4fe08f484460 242 case md_can_flame::REQUEST_RECEIVE_POSITION:
YutaTogashi 0:4fe08f484460 243 returnMessage.i = encoder.getPosition();
YutaTogashi 0:4fe08f484460 244 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_POSITION;
YutaTogashi 0:4fe08f484460 245 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 246 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 247 break;
YutaTogashi 0:4fe08f484460 248 case md_can_flame::REQUEST_RECEIVE_ROTATION:
YutaTogashi 0:4fe08f484460 249 returnMessage.f = static_cast<float>(encoder.getRotation());
YutaTogashi 0:4fe08f484460 250 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ROTATION;
YutaTogashi 0:4fe08f484460 251 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 252 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 253 break;
YutaTogashi 0:4fe08f484460 254 case md_can_flame::REQUEST_RECEIVE_RPS:
YutaTogashi 0:4fe08f484460 255 returnMessage.f = encoder.getRPS();
YutaTogashi 0:4fe08f484460 256 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPS;
YutaTogashi 0:4fe08f484460 257 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 258 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 259 break;
YutaTogashi 0:4fe08f484460 260 case md_can_flame::REQUEST_RECEIVE_RPM:
YutaTogashi 0:4fe08f484460 261 returnMessage.f = encoder.getRPM();
YutaTogashi 0:4fe08f484460 262 sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPM;
YutaTogashi 0:4fe08f484460 263 for(int i=0;i<4;i++) sendMsg.data[i+2] = returnMessage.c[i];
YutaTogashi 0:4fe08f484460 264 can.write(sendMsg);
YutaTogashi 0:4fe08f484460 265 break;
YutaTogashi 0:4fe08f484460 266 case md_can_flame::REQUEST_PRINTF_DEBAG:
YutaTogashi 0:4fe08f484460 267 printfMode = false;
YutaTogashi 0:4fe08f484460 268 break;
YutaTogashi 0:4fe08f484460 269 case md_can_flame::REQUEST_PRINTF_FORMAT_CSV:
YutaTogashi 0:4fe08f484460 270 printfMode = true;
YutaTogashi 0:4fe08f484460 271 break;
YutaTogashi 0:4fe08f484460 272 case md_can_flame::REQUEST_RESET_QEI:
YutaTogashi 0:4fe08f484460 273 encoder.reset();
YutaTogashi 0:4fe08f484460 274 break;
YutaTogashi 0:4fe08f484460 275 case md_can_flame::REQUEST_RESET_MCU:
YutaTogashi 0:4fe08f484460 276 NVIC_SystemReset();
YutaTogashi 0:4fe08f484460 277 break;
YutaTogashi 0:4fe08f484460 278 default:
YutaTogashi 0:4fe08f484460 279 break;
YutaTogashi 0:4fe08f484460 280 }
YutaTogashi 0:4fe08f484460 281 } else if(ALL_MCU == receiveMessage.id) {
YutaTogashi 0:4fe08f484460 282 NVIC_SystemReset();
YutaTogashi 0:4fe08f484460 283 } else {
YutaTogashi 0:4fe08f484460 284
YutaTogashi 0:4fe08f484460 285 }
YutaTogashi 0:4fe08f484460 286 }