Ben Katz / Hobbyking_Cheetah_Compact

Motor control for robots. More compact, less object-oriented revision.

Dependencies:   CANnucleo FastPWM3 mbed

Fork of Hobbyking_Cheetah_V1 by Ben Katz

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

Who changed what in which revision?

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