nhan pham an / Hobbyking_Cheetah_Compact_DRV8323_StepDir

Fork and fix for mwork

Dependencies:   mbed-dev-f303 FastPWM3 millis

main.cpp@22:60276ba87ac6, 2017-03-31 (annotated)

Committer:
benkatz
Date:
Fri Mar 31 18:24:46 2017 +0000
Revision:
22:60276ba87ac6
Parent:
20:bf9ea5125d52
Child:
23:2adf23ee0305
Encoder autocalibration for dc offset and harmonics

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 22:60276ba87ac6 7
benkatz 22:60276ba87ac6 8
benkatz 17:3c5df2982199 9 const unsigned int BOARDNUM = 0x2;
benkatz 17:3c5df2982199 10
benkatz 17:3c5df2982199 11 //const unsigned int a_id =
benkatz 17:3c5df2982199 12
benkatz 17:3c5df2982199 13 const unsigned int TX_ID = 0x0100;
benkatz 17:3c5df2982199 14
benkatz 18:f1d56f4acb39 15 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
benkatz 18:f1d56f4acb39 16
benkatz 17:3c5df2982199 17
benkatz 17:3c5df2982199 18
benkatz 17:3c5df2982199 19 #include "CANnucleo.h"
benkatz 0:4e1c4df6aabd 20 #include "mbed.h"
benkatz 0:4e1c4df6aabd 21 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 22 #include "structs.h"
benkatz 20:bf9ea5125d52 23 #include "foc.h"
benkatz 22:60276ba87ac6 24 #include "calibration.h"
benkatz 20:bf9ea5125d52 25 #include "hw_setup.h"
benkatz 20:bf9ea5125d52 26 #include "math_ops.h"
benkatz 20:bf9ea5125d52 27 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 28 #include "hw_config.h"
benkatz 20:bf9ea5125d52 29 #include "motor_config.h"
benkatz 9:d7eb815cb057 30
benkatz 20:bf9ea5125d52 31 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 32 ControllerStruct controller;
benkatz 20:bf9ea5125d52 33 COMStruct com;
benkatz 22:60276ba87ac6 34 VelocityEstimatorStruct velocity;
benkatz 17:3c5df2982199 35
benkatz 9:d7eb815cb057 36
benkatz 17:3c5df2982199 37
benkatz 17:3c5df2982199 38 CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 17:3c5df2982199 39 CANnucleo::CANMessage rxMsg;
benkatz 17:3c5df2982199 40 CANnucleo::CANMessage txMsg;
benkatz 17:3c5df2982199 41 int ledState;
benkatz 17:3c5df2982199 42 int counter = 0;
benkatz 18:f1d56f4acb39 43 int canCmd = 1000;
benkatz 17:3c5df2982199 44 volatile bool msgAvailable = false;
benkatz 17:3c5df2982199 45
benkatz 20:bf9ea5125d52 46 DigitalOut toggle(PA_0);
benkatz 20:bf9ea5125d52 47 Ticker loop;
benkatz 17:3c5df2982199 48 /**
benkatz 17:3c5df2982199 49 * @brief 'CAN receive-complete' interrup handler.
benkatz 17:3c5df2982199 50 * @note Called on arrival of new CAN message.
benkatz 17:3c5df2982199 51 * Keep it as short as possible.
benkatz 17:3c5df2982199 52 * @param
benkatz 17:3c5df2982199 53 * @retval
benkatz 17:3c5df2982199 54 */
benkatz 17:3c5df2982199 55 void onMsgReceived() {
benkatz 17:3c5df2982199 56 msgAvailable = true;
benkatz 17:3c5df2982199 57 //printf("ping\n\r");
benkatz 17:3c5df2982199 58 }
benkatz 17:3c5df2982199 59
benkatz 17:3c5df2982199 60 void sendCMD(int TX_addr, int val){
benkatz 17:3c5df2982199 61 txMsg.clear(); //clear Tx message storage
benkatz 17:3c5df2982199 62 txMsg.id = TX_addr;
benkatz 17:3c5df2982199 63 txMsg << val;
benkatz 17:3c5df2982199 64 can.write(txMsg);
benkatz 17:3c5df2982199 65 //wait(.1);
benkatz 17:3c5df2982199 66
benkatz 17:3c5df2982199 67 }
benkatz 17:3c5df2982199 68
benkatz 18:f1d56f4acb39 69 void readCAN(void){
benkatz 18:f1d56f4acb39 70 if(msgAvailable) {
benkatz 18:f1d56f4acb39 71 msgAvailable = false; // reset flag for next use
benkatz 18:f1d56f4acb39 72 can.read(rxMsg); // read message into Rx message storage
benkatz 18:f1d56f4acb39 73 // Filtering performed by software:
benkatz 18:f1d56f4acb39 74 if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
benkatz 18:f1d56f4acb39 75 rxMsg >> canCmd;
benkatz 18:f1d56f4acb39 76 } // extract first data item
benkatz 18:f1d56f4acb39 77 }
benkatz 18:f1d56f4acb39 78 }
benkatz 17:3c5df2982199 79
benkatz 20:bf9ea5125d52 80 void cancontroller(void){
benkatz 18:f1d56f4acb39 81 //printf("%d\n\r", canCmd);
benkatz 18:f1d56f4acb39 82 readCAN();
benkatz 18:f1d56f4acb39 83 //sendCMD(TX_ID, canCmd);
benkatz 18:f1d56f4acb39 84
benkatz 17:3c5df2982199 85 //sendCMD(TX_ID+b_ID, b1);
benkatz 17:3c5df2982199 86 //sendCMD(TX_ID+c_ID, c1);
benkatz 17:3c5df2982199 87 }
benkatz 17:3c5df2982199 88
benkatz 20:bf9ea5125d52 89
benkatz 9:d7eb815cb057 90 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 91
benkatz 22:60276ba87ac6 92 PositionSensorAM5147 spi(16384, -0.4658, NPP); ///1 I really need an eeprom or something to store this....
benkatz 22:60276ba87ac6 93 PositionSensorEncoder encoder(4096, 0, 21);
benkatz 20:bf9ea5125d52 94
benkatz 20:bf9ea5125d52 95 int count = 0;
benkatz 22:60276ba87ac6 96 int mode = 0;
benkatz 20:bf9ea5125d52 97
benkatz 22:60276ba87ac6 98
benkatz 22:60276ba87ac6 99 float velocity_estimate(void){
benkatz 22:60276ba87ac6 100 velocity.vel_2 = encoder.GetMechVelocity();
benkatz 22:60276ba87ac6 101 velocity.vel_1 = spi.GetMechVelocity();
benkatz 22:60276ba87ac6 102
benkatz 22:60276ba87ac6 103 velocity.ts = .01f;
benkatz 22:60276ba87ac6 104 velocity.vel_1_est = velocity.ts*velocity.vel_1 + (1-velocity.ts)*velocity.vel_1_est; //LPF
benkatz 22:60276ba87ac6 105 velocity.vel_2_est = (1-velocity.ts)*(velocity.vel_2_est + velocity.vel_2 - velocity.vel_2_old); //HPF
benkatz 22:60276ba87ac6 106 velocity.est = velocity.vel_1_est + velocity.vel_2_est;
benkatz 22:60276ba87ac6 107
benkatz 22:60276ba87ac6 108 velocity.vel_1_old = velocity.vel_1;
benkatz 22:60276ba87ac6 109 velocity.vel_2_old = velocity.vel_2;
benkatz 22:60276ba87ac6 110 return velocity.est;
benkatz 22:60276ba87ac6 111 }
benkatz 20:bf9ea5125d52 112
benkatz 22:60276ba87ac6 113 // Current Sampling Interrupt
benkatz 2:8724412ad628 114 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 115 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 20:bf9ea5125d52 116 //toggle = 1;
benkatz 22:60276ba87ac6 117 count++;
benkatz 22:60276ba87ac6 118 ADC1->CR2 |= 0x40000000; //Begin sample and conversion
benkatz 22:60276ba87ac6 119 //volatile int delay;
benkatz 20:bf9ea5125d52 120 //for (delay = 0; delay < 55; delay++);
benkatz 20:bf9ea5125d52 121
benkatz 22:60276ba87ac6 122 if(controller.mode == 2){
benkatz 22:60276ba87ac6 123 controller.adc2_raw = ADC2->DR;
benkatz 22:60276ba87ac6 124 controller.adc1_raw = ADC1->DR;
benkatz 22:60276ba87ac6 125
benkatz 22:60276ba87ac6 126 //toggle = 0;
benkatz 22:60276ba87ac6 127
benkatz 22:60276ba87ac6 128 spi.Sample();
benkatz 22:60276ba87ac6 129 controller.theta_elec = spi.GetElecPosition();
benkatz 22:60276ba87ac6 130 commutate(&controller, &gpio, controller.theta_elec);
benkatz 22:60276ba87ac6 131 }
benkatz 22:60276ba87ac6 132
benkatz 22:60276ba87ac6 133
benkatz 22:60276ba87ac6 134 //controller.dtheta_mech = spi.GetMechVelocity();
benkatz 22:60276ba87ac6 135 //controller.dtheta_elec = encoder.GetElecVelocity();
benkatz 22:60276ba87ac6 136 //ontroller.dtheta_mech = encoder.GetMechVelocity();
benkatz 22:60276ba87ac6 137 //controller.i_q_ref = 2.0f*controller.dtheta_mech;
benkatz 20:bf9ea5125d52 138
benkatz 22:60276ba87ac6 139
benkatz 22:60276ba87ac6 140 //float v1 = encoder.GetMechVelocity();
benkatz 22:60276ba87ac6 141 //float v2 = spi.GetMechVelocity();
benkatz 22:60276ba87ac6 142
benkatz 22:60276ba87ac6 143
benkatz 22:60276ba87ac6 144 if(count > 100){
benkatz 22:60276ba87ac6 145 count = 0;
benkatz 22:60276ba87ac6 146 //for (int i = 1; i<16; i++){
benkatz 22:60276ba87ac6 147 //pc.printf("%d\n\r ", spi.GetRawPosition());
benkatz 22:60276ba87ac6 148 // }
benkatz 22:60276ba87ac6 149 //pc.printf("\n\r");
benkatz 22:60276ba87ac6 150 //pc.printf("%.4f %.4f %.4f\n\r",velocity.vel_1 ,velocity.vel_2, velocity.est );
benkatz 22:60276ba87ac6 151
benkatz 22:60276ba87ac6 152 }
benkatz 22:60276ba87ac6 153
benkatz 22:60276ba87ac6 154
benkatz 2:8724412ad628 155 }
benkatz 2:8724412ad628 156 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 157 }
benkatz 0:4e1c4df6aabd 158
benkatz 22:60276ba87ac6 159
benkatz 22:60276ba87ac6 160 void enter_torque_mode(void){
benkatz 22:60276ba87ac6 161 controller.mode = 2;
benkatz 22:60276ba87ac6 162 TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupts
benkatz 22:60276ba87ac6 163 controller.i_d_ref = 0;
benkatz 22:60276ba87ac6 164 controller.i_q_ref = 1;
benkatz 22:60276ba87ac6 165 reset_foc(&controller); //resets integrators, and other control loop parameters
benkatz 22:60276ba87ac6 166 gpio.enable->write(1);
benkatz 22:60276ba87ac6 167 GPIOC->ODR ^= (1 << 5); //turn on LED
benkatz 22:60276ba87ac6 168 }
benkatz 22:60276ba87ac6 169
benkatz 22:60276ba87ac6 170 void enter_calibration_mode(void){
benkatz 22:60276ba87ac6 171 controller.mode = 1;
benkatz 22:60276ba87ac6 172 TIM1->CR1 ^= TIM_CR1_UDIS;
benkatz 22:60276ba87ac6 173 gpio.enable->write(1);
benkatz 22:60276ba87ac6 174 GPIOC->ODR ^= (1 << 5);
benkatz 22:60276ba87ac6 175 //calibrate_encoder(&spi);
benkatz 22:60276ba87ac6 176 order_phases(&spi, &gpio);
benkatz 22:60276ba87ac6 177 calibrate(&spi, &gpio);
benkatz 22:60276ba87ac6 178 GPIOC->ODR ^= (1 << 5);
benkatz 22:60276ba87ac6 179 wait(.2);
benkatz 22:60276ba87ac6 180 gpio.enable->write(0);
benkatz 22:60276ba87ac6 181 TIM1->CR1 ^= TIM_CR1_UDIS;
benkatz 22:60276ba87ac6 182 controller.mode = 0;
benkatz 22:60276ba87ac6 183 }
benkatz 22:60276ba87ac6 184
benkatz 0:4e1c4df6aabd 185
benkatz 0:4e1c4df6aabd 186 int main() {
benkatz 20:bf9ea5125d52 187
benkatz 20:bf9ea5125d52 188 controller.v_bus = V_BUS;
benkatz 22:60276ba87ac6 189 controller.mode = 0;
benkatz 22:60276ba87ac6 190 //spi.ZeroPosition();
benkatz 22:60276ba87ac6 191 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
benkatz 20:bf9ea5125d52 192
benkatz 9:d7eb815cb057 193 wait(.1);
benkatz 20:bf9ea5125d52 194 //TIM1->CR1 |= TIM_CR1_UDIS;
benkatz 20:bf9ea5125d52 195 gpio.enable->write(1);
benkatz 22:60276ba87ac6 196 gpio.pwm_u->write(1.0f); //write duty cycles
benkatz 20:bf9ea5125d52 197 gpio.pwm_v->write(1.0f);
benkatz 20:bf9ea5125d52 198 gpio.pwm_w->write(1.0f);
benkatz 22:60276ba87ac6 199 zero_current(&controller.adc1_offset, &controller.adc2_offset); //Measure current sensor zero-offset
benkatz 22:60276ba87ac6 200 //gpio.enable->write(0);
benkatz 20:bf9ea5125d52 201 reset_foc(&controller);
benkatz 22:60276ba87ac6 202
benkatz 22:60276ba87ac6 203 //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
benkatz 22:60276ba87ac6 204 //gpio.enable->write(1);
benkatz 22:60276ba87ac6 205 //gpio.pwm_u->write(1.0f - .05f); //write duty cycles
benkatz 22:60276ba87ac6 206 //gpio.pwm_v->write(1.0f - .05f);
benkatz 22:60276ba87ac6 207 //gpio.pwm_w->write(1.0f - .1f);
benkatz 20:bf9ea5125d52 208
benkatz 20:bf9ea5125d52 209 wait(.1);
benkatz 22:60276ba87ac6 210 NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority
benkatz 22:60276ba87ac6 211
benkatz 22:60276ba87ac6 212 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 22:60276ba87ac6 213 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 18:f1d56f4acb39 214 can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
benkatz 18:f1d56f4acb39 215
benkatz 22:60276ba87ac6 216 pc.baud(115200);
benkatz 20:bf9ea5125d52 217 wait(.01);
benkatz 20:bf9ea5125d52 218 pc.printf("HobbyKing Cheetah v1.1\n\r");
benkatz 20:bf9ea5125d52 219 pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
benkatz 20:bf9ea5125d52 220 wait(.01);
benkatz 20:bf9ea5125d52 221
benkatz 22:60276ba87ac6 222
benkatz 22:60276ba87ac6 223 enter_calibration_mode();
benkatz 22:60276ba87ac6 224 enter_torque_mode();
benkatz 20:bf9ea5125d52 225
benkatz 22:60276ba87ac6 226
benkatz 0:4e1c4df6aabd 227 while(1) {
benkatz 11:c83b18d41e54 228
benkatz 0:4e1c4df6aabd 229 }
benkatz 0:4e1c4df6aabd 230 }