Yi-Ting Hsieh / Hobbyking_Cheetah_test0223

Test the set param mode

Dependencies:   FastPWM3

main.cpp@44:8040fa2fcb0d, 2018-06-11 (annotated)

Committer:
benkatz
Date:
Mon Jun 11 00:04:06 2018 +0000
Revision:
44:8040fa2fcb0d
Parent:
43:dfb72608639c
Child:
45:26801179208e
new drv chip working;

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 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
benkatz 22:60276ba87ac6 5
benkatz 23:2adf23ee0305 6 #define REST_MODE 0
benkatz 23:2adf23ee0305 7 #define CALIBRATION_MODE 1
benkatz 26:2b865c00d7e9 8 #define MOTOR_MODE 2
benkatz 23:2adf23ee0305 9 #define SETUP_MODE 4
benkatz 23:2adf23ee0305 10 #define ENCODER_MODE 5
benkatz 22:60276ba87ac6 11
benkatz 44:8040fa2fcb0d 12 #define VERSION_NUM "1.6"
benkatz 26:2b865c00d7e9 13
benkatz 18:f1d56f4acb39 14
benkatz 26:2b865c00d7e9 15 float __float_reg[64]; // Floats stored in flash
benkatz 26:2b865c00d7e9 16 int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table
benkatz 17:3c5df2982199 17
benkatz 0:4e1c4df6aabd 18 #include "mbed.h"
benkatz 0:4e1c4df6aabd 19 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 20 #include "structs.h"
benkatz 20:bf9ea5125d52 21 #include "foc.h"
benkatz 22:60276ba87ac6 22 #include "calibration.h"
benkatz 20:bf9ea5125d52 23 #include "hw_setup.h"
benkatz 23:2adf23ee0305 24 #include "math_ops.h"
benkatz 20:bf9ea5125d52 25 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 26 #include "hw_config.h"
benkatz 20:bf9ea5125d52 27 #include "motor_config.h"
benkatz 23:2adf23ee0305 28 #include "stm32f4xx_flash.h"
benkatz 23:2adf23ee0305 29 #include "FlashWriter.h"
benkatz 23:2adf23ee0305 30 #include "user_config.h"
benkatz 23:2adf23ee0305 31 #include "PreferenceWriter.h"
benkatz 42:738fa01b0346 32 #include "CAN_com.h"
benkatz 44:8040fa2fcb0d 33 #include "DRV.h"
benkatz 26:2b865c00d7e9 34
benkatz 23:2adf23ee0305 35 PreferenceWriter prefs(6);
benkatz 9:d7eb815cb057 36
benkatz 20:bf9ea5125d52 37 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 38 ControllerStruct controller;
benkatz 20:bf9ea5125d52 39 COMStruct com;
benkatz 37:c0f352d6e8e3 40 ObserverStruct observer;
benkatz 43:dfb72608639c 41 Serial pc(PA_2, PA_3);
benkatz 9:d7eb815cb057 42
benkatz 17:3c5df2982199 43
benkatz 42:738fa01b0346 44 CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 26:2b865c00d7e9 45 CANMessage rxMsg;
benkatz 26:2b865c00d7e9 46 CANMessage txMsg;
benkatz 23:2adf23ee0305 47
benkatz 20:bf9ea5125d52 48
benkatz 44:8040fa2fcb0d 49 SPI drv_spi(PA_7, PA_6, PA_5);
benkatz 44:8040fa2fcb0d 50 DigitalOut drv_cs(PA_4);
benkatz 44:8040fa2fcb0d 51 //DigitalOut drv_en_gate(PA_11);
benkatz 44:8040fa2fcb0d 52 DRV832x drv(&drv_spi, &drv_cs);
benkatz 8:10ae7bc88d6e 53
benkatz 26:2b865c00d7e9 54 PositionSensorAM5147 spi(16384, 0.0, NPP);
benkatz 20:bf9ea5125d52 55
benkatz 23:2adf23ee0305 56 volatile int count = 0;
benkatz 23:2adf23ee0305 57 volatile int state = REST_MODE;
benkatz 23:2adf23ee0305 58 volatile int state_change;
benkatz 20:bf9ea5125d52 59
benkatz 26:2b865c00d7e9 60 void onMsgReceived() {
benkatz 26:2b865c00d7e9 61 //msgAvailable = true;
benkatz 26:2b865c00d7e9 62 //printf("%.3f %.3f %.3f\n\r", controller.theta_mech, controller.dtheta_mech, controller.i_q);
benkatz 26:2b865c00d7e9 63 can.read(rxMsg);
benkatz 28:8c7e29f719c5 64 if((rxMsg.id == CAN_ID)){
benkatz 28:8c7e29f719c5 65 controller.timeout = 0;
benkatz 28:8c7e29f719c5 66 if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){
benkatz 28:8c7e29f719c5 67 state = MOTOR_MODE;
benkatz 28:8c7e29f719c5 68 state_change = 1;
benkatz 28:8c7e29f719c5 69 }
benkatz 28:8c7e29f719c5 70 else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
benkatz 28:8c7e29f719c5 71 state = REST_MODE;
benkatz 28:8c7e29f719c5 72 state_change = 1;
benkatz 37:c0f352d6e8e3 73 gpio.led->write(0);;
benkatz 28:8c7e29f719c5 74 }
benkatz 28:8c7e29f719c5 75 else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
benkatz 28:8c7e29f719c5 76 spi.ZeroPosition();
benkatz 28:8c7e29f719c5 77 }
benkatz 28:8c7e29f719c5 78 else if(state == MOTOR_MODE){
benkatz 28:8c7e29f719c5 79 unpack_cmd(rxMsg, &controller);
benkatz 28:8c7e29f719c5 80 }
benkatz 37:c0f352d6e8e3 81 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
benkatz 37:c0f352d6e8e3 82 can.write(txMsg);
benkatz 28:8c7e29f719c5 83 }
benkatz 26:2b865c00d7e9 84
benkatz 26:2b865c00d7e9 85 }
benkatz 26:2b865c00d7e9 86
benkatz 23:2adf23ee0305 87 void enter_menu_state(void){
benkatz 44:8040fa2fcb0d 88 drv.disable_gd();
benkatz 23:2adf23ee0305 89 printf("\n\r\n\r\n\r");
benkatz 23:2adf23ee0305 90 printf(" Commands:\n\r");
benkatz 44:8040fa2fcb0d 91 wait_us(10);
benkatz 26:2b865c00d7e9 92 printf(" m - Motor Mode\n\r");
benkatz 44:8040fa2fcb0d 93 wait_us(10);
benkatz 23:2adf23ee0305 94 printf(" c - Calibrate Encoder\n\r");
benkatz 44:8040fa2fcb0d 95 wait_us(10);
benkatz 23:2adf23ee0305 96 printf(" s - Setup\n\r");
benkatz 44:8040fa2fcb0d 97 wait_us(10);
benkatz 23:2adf23ee0305 98 printf(" e - Display Encoder\n\r");
benkatz 44:8040fa2fcb0d 99 wait_us(10);
benkatz 37:c0f352d6e8e3 100 printf(" z - Set Zero Position\n\r");
benkatz 44:8040fa2fcb0d 101 wait_us(10);
benkatz 23:2adf23ee0305 102 printf(" esc - Exit to Menu\n\r");
benkatz 44:8040fa2fcb0d 103 wait_us(10);
benkatz 23:2adf23ee0305 104 state_change = 0;
benkatz 44:8040fa2fcb0d 105 //gpio.enable->write(0);
benkatz 37:c0f352d6e8e3 106 gpio.led->write(0);
benkatz 23:2adf23ee0305 107 }
benkatz 24:58c2d7571207 108
benkatz 24:58c2d7571207 109 void enter_setup_state(void){
benkatz 24:58c2d7571207 110 printf("\n\r\n\r Configuration Options \n\r\n\n");
benkatz 44:8040fa2fcb0d 111 wait_us(10);
benkatz 28:8c7e29f719c5 112 printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
benkatz 44:8040fa2fcb0d 113 wait_us(10);
benkatz 28:8c7e29f719c5 114 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
benkatz 44:8040fa2fcb0d 115 wait_us(10);
benkatz 28:8c7e29f719c5 116 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
benkatz 44:8040fa2fcb0d 117 wait_us(10);
benkatz 28:8c7e29f719c5 118 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
benkatz 44:8040fa2fcb0d 119 wait_us(10);
benkatz 28:8c7e29f719c5 120 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
benkatz 44:8040fa2fcb0d 121 wait_us(10);
benkatz 28:8c7e29f719c5 122 printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
benkatz 44:8040fa2fcb0d 123 wait_us(10);
benkatz 24:58c2d7571207 124 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
benkatz 44:8040fa2fcb0d 125 wait_us(10);
benkatz 24:58c2d7571207 126 state_change = 0;
benkatz 24:58c2d7571207 127 }
benkatz 22:60276ba87ac6 128
benkatz 23:2adf23ee0305 129 void enter_torque_mode(void){
benkatz 44:8040fa2fcb0d 130 drv.enable_gd();
benkatz 37:c0f352d6e8e3 131 controller.ovp_flag = 0;
benkatz 28:8c7e29f719c5 132 reset_foc(&controller); // Tesets integrators, and other control loop parameters
benkatz 28:8c7e29f719c5 133 wait(.001);
benkatz 23:2adf23ee0305 134 controller.i_d_ref = 0;
benkatz 28:8c7e29f719c5 135 controller.i_q_ref = 0; // Current Setpoints
benkatz 37:c0f352d6e8e3 136 gpio.led->write(1); // Turn on status LED
benkatz 25:f5741040c4bb 137 state_change = 0;
benkatz 28:8c7e29f719c5 138 printf("\n\r Entering Motor Mode \n\r");
benkatz 23:2adf23ee0305 139 }
benkatz 22:60276ba87ac6 140
benkatz 23:2adf23ee0305 141 void calibrate(void){
benkatz 44:8040fa2fcb0d 142 drv.enable_gd();
benkatz 37:c0f352d6e8e3 143 gpio.led->write(1); // Turn on status LED
benkatz 25:f5741040c4bb 144 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
benkatz 25:f5741040c4bb 145 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
benkatz 37:c0f352d6e8e3 146 gpio.led->write(0);; // Turn off status LED
benkatz 23:2adf23ee0305 147 wait(.2);
benkatz 23:2adf23ee0305 148 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
benkatz 44:8040fa2fcb0d 149 drv.disable_gd();
benkatz 23:2adf23ee0305 150 state_change = 0;
benkatz 23:2adf23ee0305 151 }
benkatz 23:2adf23ee0305 152
benkatz 23:2adf23ee0305 153 void print_encoder(void){
benkatz 23:2adf23ee0305 154 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
benkatz 23:2adf23ee0305 155 wait(.05);
benkatz 22:60276ba87ac6 156 }
benkatz 20:bf9ea5125d52 157
benkatz 23:2adf23ee0305 158 /// Current Sampling Interrupt ///
benkatz 23:2adf23ee0305 159 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
benkatz 2:8724412ad628 160 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 161 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 23:2adf23ee0305 162
benkatz 23:2adf23ee0305 163 ///Sample current always ///
benkatz 25:f5741040c4bb 164 ADC1->CR2 |= 0x40000000; // Begin sample and conversion
benkatz 22:60276ba87ac6 165 //volatile int delay;
benkatz 20:bf9ea5125d52 166 //for (delay = 0; delay < 55; delay++);
benkatz 37:c0f352d6e8e3 167 controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
benkatz 23:2adf23ee0305 168 controller.adc1_raw = ADC1->DR;
benkatz 37:c0f352d6e8e3 169 controller.adc3_raw = ADC3->DR;
benkatz 37:c0f352d6e8e3 170 spi.Sample(); // sample position sensor
benkatz 37:c0f352d6e8e3 171 controller.theta_elec = spi.GetElecPosition();
benkatz 37:c0f352d6e8e3 172 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
benkatz 37:c0f352d6e8e3 173 controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
benkatz 37:c0f352d6e8e3 174 controller.dtheta_elec = spi.GetElecVelocity();
benkatz 37:c0f352d6e8e3 175 controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
benkatz 23:2adf23ee0305 176 ///
benkatz 20:bf9ea5125d52 177
benkatz 23:2adf23ee0305 178 /// Check state machine state, and run the appropriate function ///
benkatz 23:2adf23ee0305 179 switch(state){
benkatz 37:c0f352d6e8e3 180 case REST_MODE: // Do nothing
benkatz 23:2adf23ee0305 181 if(state_change){
benkatz 23:2adf23ee0305 182 enter_menu_state();
benkatz 23:2adf23ee0305 183 }
benkatz 23:2adf23ee0305 184 break;
benkatz 22:60276ba87ac6 185
benkatz 23:2adf23ee0305 186 case CALIBRATION_MODE: // Run encoder calibration procedure
benkatz 23:2adf23ee0305 187 if(state_change){
benkatz 23:2adf23ee0305 188 calibrate();
benkatz 23:2adf23ee0305 189 }
benkatz 23:2adf23ee0305 190 break;
benkatz 23:2adf23ee0305 191
benkatz 26:2b865c00d7e9 192 case MOTOR_MODE: // Run torque control
benkatz 25:f5741040c4bb 193 if(state_change){
benkatz 25:f5741040c4bb 194 enter_torque_mode();
benkatz 28:8c7e29f719c5 195 count = 0;
benkatz 25:f5741040c4bb 196 }
benkatz 28:8c7e29f719c5 197 else{
benkatz 37:c0f352d6e8e3 198 /*
benkatz 37:c0f352d6e8e3 199 if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen
benkatz 37:c0f352d6e8e3 200 gpio.enable->write(0);
benkatz 37:c0f352d6e8e3 201 controller.ovp_flag = 1;
benkatz 37:c0f352d6e8e3 202 state = REST_MODE;
benkatz 37:c0f352d6e8e3 203 state_change = 1;
benkatz 37:c0f352d6e8e3 204 printf("OVP Triggered!\n\r");
benkatz 37:c0f352d6e8e3 205 }
benkatz 37:c0f352d6e8e3 206 */
benkatz 37:c0f352d6e8e3 207
benkatz 40:cd7e837b2b93 208 torque_control(&controller);
benkatz 28:8c7e29f719c5 209 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
benkatz 28:8c7e29f719c5 210 controller.i_d_ref = 0;
benkatz 28:8c7e29f719c5 211 controller.i_q_ref = 0;
benkatz 37:c0f352d6e8e3 212 controller.kp = 0;
benkatz 37:c0f352d6e8e3 213 controller.kd = 0;
benkatz 37:c0f352d6e8e3 214 controller.t_ff = 0;
benkatz 28:8c7e29f719c5 215 }
benkatz 44:8040fa2fcb0d 216 //commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
benkatz 44:8040fa2fcb0d 217 TIM1->CCR3 = (PWM_ARR)*(0.5f); // Write duty cycles
benkatz 44:8040fa2fcb0d 218 TIM1->CCR2 = (PWM_ARR)*(0.5f);
benkatz 44:8040fa2fcb0d 219 TIM1->CCR1 = (PWM_ARR)*(0.5f);
benkatz 28:8c7e29f719c5 220 controller.timeout += 1;
benkatz 38:67e4e1453a4b 221
benkatz 39:3580a907ef93 222 /*
benkatz 37:c0f352d6e8e3 223 count++;
benkatz 40:cd7e837b2b93 224 if(count == 4000){
benkatz 44:8040fa2fcb0d 225 //printf("%.4f\n\r", controller.dtheta_mech);
benkatz 44:8040fa2fcb0d 226
benkatz 32:ccac5da77844 227 count = 0;
benkatz 23:2adf23ee0305 228 }
benkatz 44:8040fa2fcb0d 229 */
benkatz 38:67e4e1453a4b 230
benkatz 37:c0f352d6e8e3 231
benkatz 37:c0f352d6e8e3 232 }
benkatz 23:2adf23ee0305 233 break;
benkatz 23:2adf23ee0305 234 case SETUP_MODE:
benkatz 23:2adf23ee0305 235 if(state_change){
benkatz 24:58c2d7571207 236 enter_setup_state();
benkatz 23:2adf23ee0305 237 }
benkatz 23:2adf23ee0305 238 break;
benkatz 23:2adf23ee0305 239 case ENCODER_MODE:
benkatz 23:2adf23ee0305 240 print_encoder();
benkatz 23:2adf23ee0305 241 break;
benkatz 37:c0f352d6e8e3 242 }
benkatz 2:8724412ad628 243 }
benkatz 23:2adf23ee0305 244 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 245 }
benkatz 0:4e1c4df6aabd 246
benkatz 25:f5741040c4bb 247
benkatz 24:58c2d7571207 248 char cmd_val[8] = {0};
benkatz 24:58c2d7571207 249 char cmd_id = 0;
benkatz 25:f5741040c4bb 250 char char_count = 0;
benkatz 24:58c2d7571207 251
benkatz 25:f5741040c4bb 252 /// Manage state machine with commands from serial terminal or configurator gui ///
benkatz 25:f5741040c4bb 253 /// Called when data received over serial ///
benkatz 23:2adf23ee0305 254 void serial_interrupt(void){
benkatz 23:2adf23ee0305 255 while(pc.readable()){
benkatz 23:2adf23ee0305 256 char c = pc.getc();
benkatz 25:f5741040c4bb 257 if(c == 27){
benkatz 25:f5741040c4bb 258 state = REST_MODE;
benkatz 25:f5741040c4bb 259 state_change = 1;
benkatz 25:f5741040c4bb 260 char_count = 0;
benkatz 25:f5741040c4bb 261 cmd_id = 0;
benkatz 37:c0f352d6e8e3 262 gpio.led->write(0);;
benkatz 25:f5741040c4bb 263 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 25:f5741040c4bb 264 }
benkatz 24:58c2d7571207 265 if(state == REST_MODE){
benkatz 23:2adf23ee0305 266 switch (c){
benkatz 23:2adf23ee0305 267 case 'c':
benkatz 23:2adf23ee0305 268 state = CALIBRATION_MODE;
benkatz 23:2adf23ee0305 269 state_change = 1;
benkatz 23:2adf23ee0305 270 break;
benkatz 26:2b865c00d7e9 271 case 'm':
benkatz 26:2b865c00d7e9 272 state = MOTOR_MODE;
benkatz 23:2adf23ee0305 273 state_change = 1;
benkatz 23:2adf23ee0305 274 break;
benkatz 23:2adf23ee0305 275 case 'e':
benkatz 23:2adf23ee0305 276 state = ENCODER_MODE;
benkatz 23:2adf23ee0305 277 state_change = 1;
benkatz 23:2adf23ee0305 278 break;
benkatz 23:2adf23ee0305 279 case 's':
benkatz 23:2adf23ee0305 280 state = SETUP_MODE;
benkatz 23:2adf23ee0305 281 state_change = 1;
benkatz 23:2adf23ee0305 282 break;
benkatz 37:c0f352d6e8e3 283 case 'z':
benkatz 37:c0f352d6e8e3 284 spi.SetMechOffset(0);
benkatz 37:c0f352d6e8e3 285 spi.Sample();
benkatz 37:c0f352d6e8e3 286 wait_us(20);
benkatz 37:c0f352d6e8e3 287 M_OFFSET = spi.GetMechPosition();
benkatz 37:c0f352d6e8e3 288 if (!prefs.ready()) prefs.open();
benkatz 37:c0f352d6e8e3 289 prefs.flush(); // Write new prefs to flash
benkatz 37:c0f352d6e8e3 290 prefs.close();
benkatz 37:c0f352d6e8e3 291 prefs.load();
benkatz 37:c0f352d6e8e3 292 spi.SetMechOffset(M_OFFSET);
benkatz 37:c0f352d6e8e3 293 printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
benkatz 37:c0f352d6e8e3 294
benkatz 37:c0f352d6e8e3 295 break;
benkatz 37:c0f352d6e8e3 296 }
benkatz 37:c0f352d6e8e3 297
benkatz 24:58c2d7571207 298 }
benkatz 24:58c2d7571207 299 else if(state == SETUP_MODE){
benkatz 25:f5741040c4bb 300 if(c == 13){
benkatz 24:58c2d7571207 301 switch (cmd_id){
benkatz 24:58c2d7571207 302 case 'b':
benkatz 24:58c2d7571207 303 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
benkatz 24:58c2d7571207 304 break;
benkatz 24:58c2d7571207 305 case 'i':
benkatz 24:58c2d7571207 306 CAN_ID = atoi(cmd_val);
benkatz 24:58c2d7571207 307 break;
benkatz 26:2b865c00d7e9 308 case 'm':
benkatz 26:2b865c00d7e9 309 CAN_MASTER = atoi(cmd_val);
benkatz 26:2b865c00d7e9 310 break;
benkatz 24:58c2d7571207 311 case 'l':
benkatz 24:58c2d7571207 312 TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
benkatz 24:58c2d7571207 313 break;
benkatz 28:8c7e29f719c5 314 case 't':
benkatz 28:8c7e29f719c5 315 CAN_TIMEOUT = atoi(cmd_val);
benkatz 28:8c7e29f719c5 316 break;
benkatz 24:58c2d7571207 317 default:
benkatz 24:58c2d7571207 318 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
benkatz 24:58c2d7571207 319 break;
benkatz 24:58c2d7571207 320 }
benkatz 24:58c2d7571207 321
benkatz 24:58c2d7571207 322 if (!prefs.ready()) prefs.open();
benkatz 24:58c2d7571207 323 prefs.flush(); // Write new prefs to flash
benkatz 24:58c2d7571207 324 prefs.close();
benkatz 24:58c2d7571207 325 prefs.load();
benkatz 24:58c2d7571207 326 state_change = 1;
benkatz 24:58c2d7571207 327 char_count = 0;
benkatz 24:58c2d7571207 328 cmd_id = 0;
benkatz 24:58c2d7571207 329 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 24:58c2d7571207 330 }
benkatz 24:58c2d7571207 331 else{
benkatz 24:58c2d7571207 332 if(char_count == 0){cmd_id = c;}
benkatz 24:58c2d7571207 333 else{
benkatz 24:58c2d7571207 334 cmd_val[char_count-1] = c;
benkatz 24:58c2d7571207 335
benkatz 24:58c2d7571207 336 }
benkatz 24:58c2d7571207 337 pc.putc(c);
benkatz 24:58c2d7571207 338 char_count++;
benkatz 23:2adf23ee0305 339 }
benkatz 23:2adf23ee0305 340 }
benkatz 24:58c2d7571207 341 else if (state == ENCODER_MODE){
benkatz 24:58c2d7571207 342 switch (c){
benkatz 24:58c2d7571207 343 case 27:
benkatz 24:58c2d7571207 344 state = REST_MODE;
benkatz 24:58c2d7571207 345 state_change = 1;
benkatz 24:58c2d7571207 346 break;
benkatz 24:58c2d7571207 347 }
benkatz 24:58c2d7571207 348 }
benkatz 24:58c2d7571207 349
benkatz 24:58c2d7571207 350 }
benkatz 22:60276ba87ac6 351 }
benkatz 0:4e1c4df6aabd 352
benkatz 0:4e1c4df6aabd 353 int main() {
benkatz 43:dfb72608639c 354 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 20:bf9ea5125d52 355 controller.v_bus = V_BUS;
benkatz 22:60276ba87ac6 356 controller.mode = 0;
benkatz 23:2adf23ee0305 357 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
benkatz 44:8040fa2fcb0d 358 wait(.1);
benkatz 44:8040fa2fcb0d 359
benkatz 44:8040fa2fcb0d 360 gpio.enable->write(1);
benkatz 44:8040fa2fcb0d 361 wait_us(100);
benkatz 44:8040fa2fcb0d 362 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
benkatz 44:8040fa2fcb0d 363 wait_us(100);
benkatz 44:8040fa2fcb0d 364 drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, 0x3);
benkatz 44:8040fa2fcb0d 365 wait_us(100);
benkatz 44:8040fa2fcb0d 366
benkatz 44:8040fa2fcb0d 367 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
benkatz 20:bf9ea5125d52 368
benkatz 44:8040fa2fcb0d 369
benkatz 44:8040fa2fcb0d 370
benkatz 44:8040fa2fcb0d 371
benkatz 9:d7eb815cb057 372 wait(.1);
benkatz 44:8040fa2fcb0d 373 /*
benkatz 26:2b865c00d7e9 374 gpio.enable->write(1);
benkatz 26:2b865c00d7e9 375 TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles
benkatz 26:2b865c00d7e9 376 TIM1->CCR2 = 0x708*(1.0f);
benkatz 26:2b865c00d7e9 377 TIM1->CCR1 = 0x708*(1.0f);
benkatz 26:2b865c00d7e9 378 gpio.enable->write(0);
benkatz 44:8040fa2fcb0d 379 */
benkatz 44:8040fa2fcb0d 380
benkatz 23:2adf23ee0305 381 reset_foc(&controller); // Reset current controller
benkatz 26:2b865c00d7e9 382 TIM1->CR1 ^= TIM_CR1_UDIS;
benkatz 26:2b865c00d7e9 383 //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
benkatz 20:bf9ea5125d52 384
benkatz 20:bf9ea5125d52 385 wait(.1);
benkatz 37:c0f352d6e8e3 386 NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication
benkatz 43:dfb72608639c 387
benkatz 37:c0f352d6e8e3 388 NVIC_SetPriority(CAN1_RX0_IRQn, 3);
benkatz 26:2b865c00d7e9 389 can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
benkatz 43:dfb72608639c 390
benkatz 28:8c7e29f719c5 391 txMsg.id = CAN_MASTER;
benkatz 28:8c7e29f719c5 392 txMsg.len = 6;
benkatz 26:2b865c00d7e9 393 rxMsg.len = 8;
benkatz 43:dfb72608639c 394 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 23:2adf23ee0305 395
benkatz 25:f5741040c4bb 396 prefs.load(); // Read flash
benkatz 37:c0f352d6e8e3 397 if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
benkatz 37:c0f352d6e8e3 398 if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
benkatz 25:f5741040c4bb 399 spi.SetElecOffset(E_OFFSET); // Set position sensor offset
benkatz 37:c0f352d6e8e3 400 spi.SetMechOffset(M_OFFSET);
benkatz 23:2adf23ee0305 401 int lut[128] = {0};
benkatz 23:2adf23ee0305 402 memcpy(&lut, &ENCODER_LUT, sizeof(lut));
benkatz 25:f5741040c4bb 403 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
benkatz 23:2adf23ee0305 404
benkatz 26:2b865c00d7e9 405 pc.baud(921600); // set serial baud rate
benkatz 20:bf9ea5125d52 406 wait(.01);
benkatz 23:2adf23ee0305 407 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
benkatz 20:bf9ea5125d52 408 wait(.01);
benkatz 23:2adf23ee0305 409 printf("\n\r Debug Info:\n\r");
benkatz 32:ccac5da77844 410 printf(" Firmware Version: %s\n\r", VERSION_NUM);
benkatz 23:2adf23ee0305 411 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
benkatz 23:2adf23ee0305 412 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
benkatz 37:c0f352d6e8e3 413 printf(" Output Zero Position: %.4f\n\r", M_OFFSET);
benkatz 24:58c2d7571207 414 printf(" CAN ID: %d\n\r", CAN_ID);
benkatz 44:8040fa2fcb0d 415
benkatz 44:8040fa2fcb0d 416
benkatz 44:8040fa2fcb0d 417
benkatz 44:8040fa2fcb0d 418
benkatz 44:8040fa2fcb0d 419 printf(" %d\n\r", drv.read_register(DCR));
benkatz 44:8040fa2fcb0d 420 wait_us(100);
benkatz 44:8040fa2fcb0d 421 printf(" %d\n\r", drv.read_register(CSACR));
benkatz 44:8040fa2fcb0d 422
benkatz 44:8040fa2fcb0d 423 drv.disable_gd();
benkatz 44:8040fa2fcb0d 424
benkatz 23:2adf23ee0305 425 pc.attach(&serial_interrupt); // attach serial interrupt
benkatz 22:60276ba87ac6 426
benkatz 23:2adf23ee0305 427 state_change = 1;
benkatz 44:8040fa2fcb0d 428
benkatz 20:bf9ea5125d52 429
benkatz 44:8040fa2fcb0d 430 int counter = 0;
benkatz 0:4e1c4df6aabd 431 while(1) {
benkatz 44:8040fa2fcb0d 432 counter++;
benkatz 44:8040fa2fcb0d 433 if(counter>40000)
benkatz 44:8040fa2fcb0d 434 {
benkatz 44:8040fa2fcb0d 435 //gpio.enable->write(1);
benkatz 44:8040fa2fcb0d 436 //wait_us(100);
benkatz 44:8040fa2fcb0d 437 //printf(" %d\n\r", drv.read_register(DCR));
benkatz 44:8040fa2fcb0d 438 //wait_us(100);
benkatz 44:8040fa2fcb0d 439 //printf(" %d\n\r", drv.read_register(CSACR));
benkatz 44:8040fa2fcb0d 440 drv.print_faults();
benkatz 44:8040fa2fcb0d 441 //printf("%d\n\r", drv.read_register(DCR));
benkatz 44:8040fa2fcb0d 442 counter = 0;
benkatz 44:8040fa2fcb0d 443 //gpio.enable->write(0);
benkatz 44:8040fa2fcb0d 444 }
benkatz 44:8040fa2fcb0d 445 wait_us(25);
benkatz 0:4e1c4df6aabd 446 }
benkatz 0:4e1c4df6aabd 447 }