Shao Rui / Joint_control_2AS5147_DRV8323RH_GR49_HJB_2020_8_10

1

Dependencies:   mbed-dev-f303 FastPWM3

main.cpp@23:2adf23ee0305, 2017-04-05 (annotated)

Committer:
benkatz
Date:
Wed Apr 05 20:54:16 2017 +0000
Revision:
23:2adf23ee0305
Parent:
22:60276ba87ac6
Child:
24:58c2d7571207
Added bayley's flash writer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 22:60276ba87ac6 1 /// high-bandwidth 3-phase motor control, for robots
benkatz 22:60276ba87ac6 2 /// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
benkatz 22:60276ba87ac6 3 /// Hardware documentation can be found at build-its.blogspot.com
benkatz 22:60276ba87ac6 4
benkatz 22:60276ba87ac6 5 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
benkatz 22:60276ba87ac6 6
benkatz 23:2adf23ee0305 7 #define REST_MODE 0
benkatz 23:2adf23ee0305 8 #define CALIBRATION_MODE 1
benkatz 23:2adf23ee0305 9 #define TORQUE_MODE 2
benkatz 23:2adf23ee0305 10 #define PD_MODE 3
benkatz 23:2adf23ee0305 11 #define SETUP_MODE 4
benkatz 23:2adf23ee0305 12 #define ENCODER_MODE 5
benkatz 22:60276ba87ac6 13
benkatz 22:60276ba87ac6 14
benkatz 17:3c5df2982199 15 const unsigned int BOARDNUM = 0x2;
benkatz 17:3c5df2982199 16 //const unsigned int a_id =
benkatz 17:3c5df2982199 17 const unsigned int TX_ID = 0x0100;
benkatz 18:f1d56f4acb39 18 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
benkatz 18:f1d56f4acb39 19
benkatz 23:2adf23ee0305 20 float __float_reg[64];
benkatz 23:2adf23ee0305 21 int __int_reg[256];
benkatz 17:3c5df2982199 22
benkatz 17:3c5df2982199 23 #include "CANnucleo.h"
benkatz 0:4e1c4df6aabd 24 #include "mbed.h"
benkatz 0:4e1c4df6aabd 25 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 26 #include "structs.h"
benkatz 20:bf9ea5125d52 27 #include "foc.h"
benkatz 22:60276ba87ac6 28 #include "calibration.h"
benkatz 20:bf9ea5125d52 29 #include "hw_setup.h"
benkatz 23:2adf23ee0305 30 #include "math_ops.h"
benkatz 20:bf9ea5125d52 31 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 32 #include "hw_config.h"
benkatz 20:bf9ea5125d52 33 #include "motor_config.h"
benkatz 23:2adf23ee0305 34 #include "stm32f4xx_flash.h"
benkatz 23:2adf23ee0305 35 #include "FlashWriter.h"
benkatz 23:2adf23ee0305 36 #include "user_config.h"
benkatz 23:2adf23ee0305 37 #include "PreferenceWriter.h"
benkatz 23:2adf23ee0305 38 //#include "state_machine.h"
benkatz 23:2adf23ee0305 39
benkatz 23:2adf23ee0305 40
benkatz 23:2adf23ee0305 41
benkatz 23:2adf23ee0305 42 PreferenceWriter prefs(6);
benkatz 9:d7eb815cb057 43
benkatz 20:bf9ea5125d52 44 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 45 ControllerStruct controller;
benkatz 20:bf9ea5125d52 46 COMStruct com;
benkatz 22:60276ba87ac6 47 VelocityEstimatorStruct velocity;
benkatz 17:3c5df2982199 48
benkatz 9:d7eb815cb057 49
benkatz 17:3c5df2982199 50
benkatz 23:2adf23ee0305 51 CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 17:3c5df2982199 52 CANnucleo::CANMessage rxMsg;
benkatz 17:3c5df2982199 53 CANnucleo::CANMessage txMsg;
benkatz 17:3c5df2982199 54 int ledState;
benkatz 17:3c5df2982199 55 int counter = 0;
benkatz 18:f1d56f4acb39 56 int canCmd = 1000;
benkatz 17:3c5df2982199 57 volatile bool msgAvailable = false;
benkatz 17:3c5df2982199 58
benkatz 20:bf9ea5125d52 59 DigitalOut toggle(PA_0);
benkatz 20:bf9ea5125d52 60 Ticker loop;
benkatz 17:3c5df2982199 61 /**
benkatz 17:3c5df2982199 62 * @brief 'CAN receive-complete' interrup handler.
benkatz 17:3c5df2982199 63 * @note Called on arrival of new CAN message.
benkatz 17:3c5df2982199 64 * Keep it as short as possible.
benkatz 17:3c5df2982199 65 * @param
benkatz 17:3c5df2982199 66 * @retval
benkatz 17:3c5df2982199 67 */
benkatz 17:3c5df2982199 68 void onMsgReceived() {
benkatz 17:3c5df2982199 69 msgAvailable = true;
benkatz 17:3c5df2982199 70 //printf("ping\n\r");
benkatz 17:3c5df2982199 71 }
benkatz 17:3c5df2982199 72
benkatz 17:3c5df2982199 73 void sendCMD(int TX_addr, int val){
benkatz 17:3c5df2982199 74 txMsg.clear(); //clear Tx message storage
benkatz 17:3c5df2982199 75 txMsg.id = TX_addr;
benkatz 17:3c5df2982199 76 txMsg << val;
benkatz 17:3c5df2982199 77 can.write(txMsg);
benkatz 17:3c5df2982199 78 //wait(.1);
benkatz 17:3c5df2982199 79
benkatz 17:3c5df2982199 80 }
benkatz 17:3c5df2982199 81
benkatz 18:f1d56f4acb39 82 void readCAN(void){
benkatz 18:f1d56f4acb39 83 if(msgAvailable) {
benkatz 23:2adf23ee0305 84 msgAvailable = false; // reset flag for next use
benkatz 23:2adf23ee0305 85 can.read(rxMsg); // read message into Rx message storage
benkatz 18:f1d56f4acb39 86 // Filtering performed by software:
benkatz 23:2adf23ee0305 87 if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
benkatz 23:2adf23ee0305 88 rxMsg >> canCmd; // extract first data item
benkatz 23:2adf23ee0305 89 }
benkatz 18:f1d56f4acb39 90 }
benkatz 23:2adf23ee0305 91 }
benkatz 17:3c5df2982199 92
benkatz 20:bf9ea5125d52 93 void cancontroller(void){
benkatz 18:f1d56f4acb39 94 //printf("%d\n\r", canCmd);
benkatz 18:f1d56f4acb39 95 readCAN();
benkatz 18:f1d56f4acb39 96 //sendCMD(TX_ID, canCmd);
benkatz 23:2adf23ee0305 97
benkatz 17:3c5df2982199 98 }
benkatz 17:3c5df2982199 99
benkatz 20:bf9ea5125d52 100
benkatz 9:d7eb815cb057 101 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 102
benkatz 23:2adf23ee0305 103 PositionSensorAM5147 spi(16384, 0.0, NPP);
benkatz 22:60276ba87ac6 104 PositionSensorEncoder encoder(4096, 0, 21);
benkatz 20:bf9ea5125d52 105
benkatz 23:2adf23ee0305 106 volatile int count = 0;
benkatz 23:2adf23ee0305 107 volatile int state = REST_MODE;
benkatz 23:2adf23ee0305 108 volatile int state_change;
benkatz 20:bf9ea5125d52 109
benkatz 23:2adf23ee0305 110 void enter_menu_state(void){
benkatz 23:2adf23ee0305 111 printf("\n\r\n\r\n\r");
benkatz 23:2adf23ee0305 112 printf(" Commands:\n\r");
benkatz 23:2adf23ee0305 113 printf(" t - Torque Mode\n\r");
benkatz 23:2adf23ee0305 114 printf(" p - PD Mode\n\r");
benkatz 23:2adf23ee0305 115 printf(" c - Calibrate Encoder\n\r");
benkatz 23:2adf23ee0305 116 printf(" s - Setup\n\r");
benkatz 23:2adf23ee0305 117 printf(" e - Display Encoder\n\r");
benkatz 23:2adf23ee0305 118 printf(" esc - Exit to Menu\n\r");
benkatz 23:2adf23ee0305 119 state_change = 0;
benkatz 23:2adf23ee0305 120 }
benkatz 22:60276ba87ac6 121
benkatz 23:2adf23ee0305 122 void enter_torque_mode(void){
benkatz 23:2adf23ee0305 123 controller.mode = 2;
benkatz 23:2adf23ee0305 124 controller.i_d_ref = 0;
benkatz 23:2adf23ee0305 125 controller.i_q_ref = 0;
benkatz 23:2adf23ee0305 126 reset_foc(&controller); //resets integrators, and other control loop parameters
benkatz 23:2adf23ee0305 127 gpio.enable->write(1);
benkatz 23:2adf23ee0305 128 GPIOC->ODR ^= (1 << 5); //turn on status LED
benkatz 23:2adf23ee0305 129 }
benkatz 22:60276ba87ac6 130
benkatz 23:2adf23ee0305 131 void calibrate(void){
benkatz 23:2adf23ee0305 132 gpio.enable->write(1);
benkatz 23:2adf23ee0305 133 GPIOC->ODR ^= (1 << 5);
benkatz 23:2adf23ee0305 134 order_phases(&spi, &gpio, &controller, &prefs);
benkatz 23:2adf23ee0305 135 calibrate(&spi, &gpio, &controller, &prefs);
benkatz 23:2adf23ee0305 136 GPIOC->ODR ^= (1 << 5);
benkatz 23:2adf23ee0305 137 wait(.2);
benkatz 23:2adf23ee0305 138 gpio.enable->write(0);
benkatz 23:2adf23ee0305 139 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
benkatz 23:2adf23ee0305 140 state_change = 0;
benkatz 23:2adf23ee0305 141
benkatz 23:2adf23ee0305 142 }
benkatz 23:2adf23ee0305 143
benkatz 23:2adf23ee0305 144 void print_encoder(void){
benkatz 23:2adf23ee0305 145 spi.Sample();
benkatz 23:2adf23ee0305 146 wait(.001);
benkatz 23:2adf23ee0305 147 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
benkatz 23:2adf23ee0305 148 wait(.05);
benkatz 22:60276ba87ac6 149 }
benkatz 20:bf9ea5125d52 150
benkatz 23:2adf23ee0305 151 /// Current Sampling Interrupt ///
benkatz 23:2adf23ee0305 152 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
benkatz 2:8724412ad628 153 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 154 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 20:bf9ea5125d52 155 //toggle = 1;
benkatz 23:2adf23ee0305 156
benkatz 23:2adf23ee0305 157 ///Sample current always ///
benkatz 23:2adf23ee0305 158 ADC1->CR2 |= 0x40000000; //Begin sample and conversion
benkatz 22:60276ba87ac6 159 //volatile int delay;
benkatz 20:bf9ea5125d52 160 //for (delay = 0; delay < 55; delay++);
benkatz 23:2adf23ee0305 161 controller.adc2_raw = ADC2->DR;
benkatz 23:2adf23ee0305 162 controller.adc1_raw = ADC1->DR;
benkatz 23:2adf23ee0305 163 ///
benkatz 20:bf9ea5125d52 164
benkatz 23:2adf23ee0305 165 /// Check state machine state, and run the appropriate function ///
benkatz 23:2adf23ee0305 166 //printf("%d\n\r", state);
benkatz 23:2adf23ee0305 167 switch(state){
benkatz 23:2adf23ee0305 168 case REST_MODE: // Do nothing until
benkatz 23:2adf23ee0305 169 if(state_change){
benkatz 23:2adf23ee0305 170 enter_menu_state();
benkatz 23:2adf23ee0305 171 }
benkatz 23:2adf23ee0305 172 break;
benkatz 22:60276ba87ac6 173
benkatz 23:2adf23ee0305 174 case CALIBRATION_MODE: // Run encoder calibration procedure
benkatz 23:2adf23ee0305 175 if(state_change){
benkatz 23:2adf23ee0305 176 calibrate();
benkatz 23:2adf23ee0305 177 }
benkatz 23:2adf23ee0305 178 break;
benkatz 23:2adf23ee0305 179
benkatz 23:2adf23ee0305 180 case TORQUE_MODE: // Run torque control
benkatz 23:2adf23ee0305 181 count++;
benkatz 23:2adf23ee0305 182 controller.theta_elec = spi.GetElecPosition();
benkatz 23:2adf23ee0305 183 commutate(&controller, &gpio, controller.theta_elec); // Run current loop
benkatz 23:2adf23ee0305 184 spi.Sample(); // Sample position sensor
benkatz 23:2adf23ee0305 185 if(count > 100){
benkatz 23:2adf23ee0305 186 count = 0;
benkatz 23:2adf23ee0305 187 readCAN();
benkatz 23:2adf23ee0305 188 controller.i_q_ref = ((float)(canCmd-1000))/100;
benkatz 23:2adf23ee0305 189 //pc.printf("%f\n\r ", controller.theta_elec);
benkatz 23:2adf23ee0305 190 }
benkatz 23:2adf23ee0305 191 break;
benkatz 23:2adf23ee0305 192
benkatz 23:2adf23ee0305 193 case PD_MODE:
benkatz 23:2adf23ee0305 194 break;
benkatz 23:2adf23ee0305 195 case SETUP_MODE:
benkatz 23:2adf23ee0305 196 if(state_change){
benkatz 23:2adf23ee0305 197 printf("\n\r Configuration Menu \n\r\n\n");
benkatz 23:2adf23ee0305 198 state_change = 0;
benkatz 23:2adf23ee0305 199 }
benkatz 23:2adf23ee0305 200 break;
benkatz 23:2adf23ee0305 201 case ENCODER_MODE:
benkatz 23:2adf23ee0305 202 print_encoder();
benkatz 23:2adf23ee0305 203 break;
benkatz 23:2adf23ee0305 204 }
benkatz 23:2adf23ee0305 205
benkatz 23:2adf23ee0305 206
benkatz 22:60276ba87ac6 207
benkatz 2:8724412ad628 208 }
benkatz 23:2adf23ee0305 209 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 210 }
benkatz 0:4e1c4df6aabd 211
benkatz 23:2adf23ee0305 212 /// Manage state machine with commands from serial terminal or configurator gui ///
benkatz 23:2adf23ee0305 213 void serial_interrupt(void){
benkatz 23:2adf23ee0305 214 while(pc.readable()){
benkatz 23:2adf23ee0305 215 char c = pc.getc();
benkatz 23:2adf23ee0305 216 if(c == 27){
benkatz 23:2adf23ee0305 217 state = REST_MODE;
benkatz 23:2adf23ee0305 218 state_change = 1;
benkatz 23:2adf23ee0305 219 }
benkatz 23:2adf23ee0305 220 else if(state == REST_MODE){
benkatz 23:2adf23ee0305 221 switch (c){
benkatz 23:2adf23ee0305 222 case 'c':
benkatz 23:2adf23ee0305 223 state = CALIBRATION_MODE;
benkatz 23:2adf23ee0305 224 state_change = 1;
benkatz 23:2adf23ee0305 225 break;
benkatz 23:2adf23ee0305 226 case 't':
benkatz 23:2adf23ee0305 227 state = TORQUE_MODE;
benkatz 23:2adf23ee0305 228 state_change = 1;
benkatz 23:2adf23ee0305 229 break;
benkatz 23:2adf23ee0305 230 case 'e':
benkatz 23:2adf23ee0305 231 state = ENCODER_MODE;
benkatz 23:2adf23ee0305 232 state_change = 1;
benkatz 23:2adf23ee0305 233 break;
benkatz 23:2adf23ee0305 234 case 's':
benkatz 23:2adf23ee0305 235 state = SETUP_MODE;
benkatz 23:2adf23ee0305 236 state_change = 1;
benkatz 23:2adf23ee0305 237 break;
benkatz 23:2adf23ee0305 238
benkatz 23:2adf23ee0305 239 }
benkatz 23:2adf23ee0305 240 }
benkatz 22:60276ba87ac6 241 }
benkatz 22:60276ba87ac6 242 }
benkatz 0:4e1c4df6aabd 243
benkatz 0:4e1c4df6aabd 244 int main() {
benkatz 23:2adf23ee0305 245
benkatz 23:2adf23ee0305 246
benkatz 20:bf9ea5125d52 247
benkatz 20:bf9ea5125d52 248 controller.v_bus = V_BUS;
benkatz 22:60276ba87ac6 249 controller.mode = 0;
benkatz 22:60276ba87ac6 250 //spi.ZeroPosition();
benkatz 23:2adf23ee0305 251 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
benkatz 20:bf9ea5125d52 252
benkatz 9:d7eb815cb057 253 wait(.1);
benkatz 20:bf9ea5125d52 254 //TIM1->CR1 |= TIM_CR1_UDIS;
benkatz 23:2adf23ee0305 255 gpio.enable->write(1); // Enable gate drive
benkatz 23:2adf23ee0305 256 gpio.pwm_u->write(1.0f); // Write duty cycles
benkatz 20:bf9ea5125d52 257 gpio.pwm_v->write(1.0f);
benkatz 20:bf9ea5125d52 258 gpio.pwm_w->write(1.0f);
benkatz 23:2adf23ee0305 259 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
benkatz 22:60276ba87ac6 260 //gpio.enable->write(0);
benkatz 23:2adf23ee0305 261 reset_foc(&controller); // Reset current controller
benkatz 22:60276ba87ac6 262
benkatz 23:2adf23ee0305 263 TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
benkatz 20:bf9ea5125d52 264
benkatz 20:bf9ea5125d52 265 wait(.1);
benkatz 23:2adf23ee0305 266 NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority
benkatz 22:60276ba87ac6 267
benkatz 23:2adf23ee0305 268 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 23:2adf23ee0305 269 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 18:f1d56f4acb39 270 can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
benkatz 18:f1d56f4acb39 271
benkatz 23:2adf23ee0305 272
benkatz 23:2adf23ee0305 273 prefs.load();
benkatz 23:2adf23ee0305 274 spi.SetElecOffset(E_OFFSET);
benkatz 23:2adf23ee0305 275 int lut[128] = {0};
benkatz 23:2adf23ee0305 276 memcpy(&lut, &ENCODER_LUT, sizeof(lut));
benkatz 23:2adf23ee0305 277 spi.WriteLUT(lut);
benkatz 23:2adf23ee0305 278
benkatz 23:2adf23ee0305 279 pc.baud(115200); // set serial baud rate
benkatz 20:bf9ea5125d52 280 wait(.01);
benkatz 23:2adf23ee0305 281 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
benkatz 20:bf9ea5125d52 282 wait(.01);
benkatz 23:2adf23ee0305 283 printf("\n\r Debug Info:\n\r");
benkatz 23:2adf23ee0305 284 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
benkatz 23:2adf23ee0305 285 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
benkatz 23:2adf23ee0305 286 printf(" CAN ID: %d\n\r", BOARDNUM);
benkatz 23:2adf23ee0305 287
benkatz 23:2adf23ee0305 288 pc.attach(&serial_interrupt); // attach serial interrupt
benkatz 22:60276ba87ac6 289
benkatz 23:2adf23ee0305 290 state_change = 1;
benkatz 23:2adf23ee0305 291 //enter_menu_state(); //Print available commands, wait for command
benkatz 23:2adf23ee0305 292 //enter_calibration_mode();
benkatz 23:2adf23ee0305 293 //wait_us(100);
benkatz 23:2adf23ee0305 294
benkatz 23:2adf23ee0305 295 //enter_torque_mode();
benkatz 20:bf9ea5125d52 296
benkatz 22:60276ba87ac6 297
benkatz 0:4e1c4df6aabd 298 while(1) {
benkatz 11:c83b18d41e54 299
benkatz 0:4e1c4df6aabd 300 }
benkatz 0:4e1c4df6aabd 301 }