Shao Rui / test_motor_output_position

1

Dependencies:   mbed-dev-f303 FastPWM3

main.cpp@20:bf9ea5125d52, 2017-03-02 (annotated)

Committer:
benkatz
Date:
Thu Mar 02 15:31:23 2017 +0000
Revision:
20:bf9ea5125d52
Parent:
19:bd10a04eedc2
Child:
22:60276ba87ac6
Compact version works.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 17:3c5df2982199 1 const unsigned int BOARDNUM = 0x2;
benkatz 17:3c5df2982199 2
benkatz 17:3c5df2982199 3 //const unsigned int a_id =
benkatz 17:3c5df2982199 4
benkatz 17:3c5df2982199 5 const unsigned int TX_ID = 0x0100;
benkatz 17:3c5df2982199 6
benkatz 18:f1d56f4acb39 7 const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
benkatz 18:f1d56f4acb39 8
benkatz 17:3c5df2982199 9
benkatz 17:3c5df2982199 10
benkatz 17:3c5df2982199 11 #include "CANnucleo.h"
benkatz 0:4e1c4df6aabd 12 #include "mbed.h"
benkatz 0:4e1c4df6aabd 13 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 14 #include "structs.h"
benkatz 20:bf9ea5125d52 15 #include "foc.h"
benkatz 20:bf9ea5125d52 16 #include "hw_setup.h"
benkatz 20:bf9ea5125d52 17 #include "math_ops.h"
benkatz 20:bf9ea5125d52 18 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 19 #include "hw_config.h"
benkatz 20:bf9ea5125d52 20 #include "motor_config.h"
benkatz 9:d7eb815cb057 21
benkatz 20:bf9ea5125d52 22 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 23 ControllerStruct controller;
benkatz 20:bf9ea5125d52 24 COMStruct com;
benkatz 17:3c5df2982199 25
benkatz 9:d7eb815cb057 26
benkatz 17:3c5df2982199 27
benkatz 17:3c5df2982199 28 CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 17:3c5df2982199 29 CANnucleo::CANMessage rxMsg;
benkatz 17:3c5df2982199 30 CANnucleo::CANMessage txMsg;
benkatz 17:3c5df2982199 31 int ledState;
benkatz 17:3c5df2982199 32 int counter = 0;
benkatz 18:f1d56f4acb39 33 int canCmd = 1000;
benkatz 17:3c5df2982199 34 volatile bool msgAvailable = false;
benkatz 17:3c5df2982199 35
benkatz 20:bf9ea5125d52 36 DigitalOut toggle(PA_0);
benkatz 20:bf9ea5125d52 37 Ticker loop;
benkatz 17:3c5df2982199 38 /**
benkatz 17:3c5df2982199 39 * @brief 'CAN receive-complete' interrup handler.
benkatz 17:3c5df2982199 40 * @note Called on arrival of new CAN message.
benkatz 17:3c5df2982199 41 * Keep it as short as possible.
benkatz 17:3c5df2982199 42 * @param
benkatz 17:3c5df2982199 43 * @retval
benkatz 17:3c5df2982199 44 */
benkatz 17:3c5df2982199 45 void onMsgReceived() {
benkatz 17:3c5df2982199 46 msgAvailable = true;
benkatz 17:3c5df2982199 47 //printf("ping\n\r");
benkatz 17:3c5df2982199 48 }
benkatz 17:3c5df2982199 49
benkatz 17:3c5df2982199 50 void sendCMD(int TX_addr, int val){
benkatz 17:3c5df2982199 51 txMsg.clear(); //clear Tx message storage
benkatz 17:3c5df2982199 52 txMsg.id = TX_addr;
benkatz 17:3c5df2982199 53 txMsg << val;
benkatz 17:3c5df2982199 54 can.write(txMsg);
benkatz 17:3c5df2982199 55 //wait(.1);
benkatz 17:3c5df2982199 56
benkatz 17:3c5df2982199 57 }
benkatz 17:3c5df2982199 58
benkatz 18:f1d56f4acb39 59 void readCAN(void){
benkatz 18:f1d56f4acb39 60 if(msgAvailable) {
benkatz 18:f1d56f4acb39 61 msgAvailable = false; // reset flag for next use
benkatz 18:f1d56f4acb39 62 can.read(rxMsg); // read message into Rx message storage
benkatz 18:f1d56f4acb39 63 // Filtering performed by software:
benkatz 18:f1d56f4acb39 64 if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
benkatz 18:f1d56f4acb39 65 rxMsg >> canCmd;
benkatz 18:f1d56f4acb39 66 } // extract first data item
benkatz 18:f1d56f4acb39 67 }
benkatz 18:f1d56f4acb39 68 }
benkatz 17:3c5df2982199 69
benkatz 20:bf9ea5125d52 70 void cancontroller(void){
benkatz 18:f1d56f4acb39 71 //printf("%d\n\r", canCmd);
benkatz 18:f1d56f4acb39 72 readCAN();
benkatz 18:f1d56f4acb39 73 //sendCMD(TX_ID, canCmd);
benkatz 18:f1d56f4acb39 74
benkatz 17:3c5df2982199 75 //sendCMD(TX_ID+b_ID, b1);
benkatz 17:3c5df2982199 76 //sendCMD(TX_ID+c_ID, c1);
benkatz 17:3c5df2982199 77 }
benkatz 17:3c5df2982199 78
benkatz 20:bf9ea5125d52 79
benkatz 9:d7eb815cb057 80 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 81
benkatz 20:bf9ea5125d52 82 PositionSensorAM5147 spi(16384, 4.7, NPP); ///1 I really need an eeprom or something to store this....
benkatz 20:bf9ea5125d52 83 //PositionSensorEncoder encoder(4096, 0, 21);
benkatz 20:bf9ea5125d52 84
benkatz 20:bf9ea5125d52 85 int count = 0;
benkatz 20:bf9ea5125d52 86 void commutate(void){
benkatz 20:bf9ea5125d52 87
benkatz 20:bf9ea5125d52 88 count ++;
benkatz 20:bf9ea5125d52 89
benkatz 20:bf9ea5125d52 90 //pc.printf("%f\n\r", controller.theta_elec);
benkatz 20:bf9ea5125d52 91 //Get rotor angle
benkatz 20:bf9ea5125d52 92 //spi.GetMechPosition();
benkatz 20:bf9ea5125d52 93 controller.i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset); //Calculate phase currents from ADC readings
benkatz 20:bf9ea5125d52 94 controller.i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset);
benkatz 20:bf9ea5125d52 95 controller.i_a = -controller.i_b - controller.i_c;
benkatz 20:bf9ea5125d52 96
benkatz 20:bf9ea5125d52 97
benkatz 20:bf9ea5125d52 98 dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q); //dq0 transform on currents
benkatz 20:bf9ea5125d52 99
benkatz 20:bf9ea5125d52 100 ///Control Law///
benkatz 20:bf9ea5125d52 101 float i_d_error = controller.i_d_ref - controller.i_d;
benkatz 20:bf9ea5125d52 102 float i_q_error = controller.i_q_ref - controller.i_q;
benkatz 20:bf9ea5125d52 103 float v_d_ff = controller.i_d_ref*R_TOTAL; //feed-forward voltage
benkatz 20:bf9ea5125d52 104 float v_q_ff = controller.i_q_ref*R_TOTAL;
benkatz 20:bf9ea5125d52 105 controller.d_int += i_d_error;
benkatz 20:bf9ea5125d52 106 controller.q_int += i_q_error;
benkatz 20:bf9ea5125d52 107
benkatz 20:bf9ea5125d52 108 limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q));
benkatz 20:bf9ea5125d52 109 //controller.d_int = fminf(fmaxf(controller.d_int, -D_INT_LIM), D_INT_LIM);
benkatz 20:bf9ea5125d52 110 //controller.q_int = fminf(fmaxf(controller.q_int, -Q_INT_LIM), Q_INT_LIM);
benkatz 20:bf9ea5125d52 111
benkatz 20:bf9ea5125d52 112
benkatz 20:bf9ea5125d52 113 controller.v_d = K_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff;
benkatz 20:bf9ea5125d52 114 controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff;
benkatz 20:bf9ea5125d52 115 //controller.v_d = 10*v_d_ff;
benkatz 20:bf9ea5125d52 116 //controller.v_q = 10*v_q_ff;
benkatz 20:bf9ea5125d52 117 limit_norm(&controller.v_d, &controller.v_q, controller.v_bus);
benkatz 20:bf9ea5125d52 118
benkatz 20:bf9ea5125d52 119 abc(controller.theta_elec, controller.v_d, controller.v_q, &controller.v_u, &controller.v_v, &controller.v_w); //inverse dq0 transform on voltages
benkatz 20:bf9ea5125d52 120 svm(controller.v_bus, controller.v_u, controller.v_v, controller.v_w, &controller.dtc_u, &controller.dtc_v, &controller.dtc_w); //space vector modulation
benkatz 20:bf9ea5125d52 121
benkatz 20:bf9ea5125d52 122 gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles
benkatz 20:bf9ea5125d52 123 gpio.pwm_v->write(1.0f-controller.dtc_v);
benkatz 20:bf9ea5125d52 124 gpio.pwm_w->write(1.0f-controller.dtc_w);
benkatz 20:bf9ea5125d52 125
benkatz 20:bf9ea5125d52 126 controller.theta_elec = spi.GetElecPosition();
benkatz 20:bf9ea5125d52 127 //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles
benkatz 20:bf9ea5125d52 128 //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v);
benkatz 20:bf9ea5125d52 129 //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w);
benkatz 20:bf9ea5125d52 130
benkatz 20:bf9ea5125d52 131 //gpio.pwm_u->write(1.0f - .1f); //write duty cycles
benkatz 20:bf9ea5125d52 132 //gpio.pwm_v->write(1.0f - .1f);
benkatz 20:bf9ea5125d52 133 //gpio.pwm_w->write(1.0f - .15f);
benkatz 14:80ce59119d93 134
benkatz 9:d7eb815cb057 135
benkatz 20:bf9ea5125d52 136 if(count >1000){
benkatz 20:bf9ea5125d52 137 controller.i_q_ref = -controller.i_q_ref;
benkatz 20:bf9ea5125d52 138 count = 0;
benkatz 20:bf9ea5125d52 139 //pc.printf("%f\n\r", controller.theta_elec);
benkatz 20:bf9ea5125d52 140 //pc.printf("%f %f %f\n\r", controller.i_a, controller.i_b, controller.i_c);
benkatz 20:bf9ea5125d52 141 //pc.printf("%f %f\n\r", controller.i_d, controller.i_q);
benkatz 20:bf9ea5125d52 142 //pc.printf("%d %d\n\r", controller.adc1_raw, controller.adc2_raw);
benkatz 20:bf9ea5125d52 143 }
benkatz 20:bf9ea5125d52 144 }
benkatz 20:bf9ea5125d52 145
benkatz 0:4e1c4df6aabd 146
benkatz 1:b8bceb4daed5 147 // Current Sampling IRQ
benkatz 2:8724412ad628 148 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 149 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 20:bf9ea5125d52 150 //toggle = 1;
benkatz 20:bf9ea5125d52 151 ADC1->CR2 |= 0x40000000;
benkatz 20:bf9ea5125d52 152 //volatile int delay;
benkatz 20:bf9ea5125d52 153 //for (delay = 0; delay < 55; delay++);
benkatz 20:bf9ea5125d52 154
benkatz 20:bf9ea5125d52 155 controller.adc2_raw = ADC2->DR;
benkatz 20:bf9ea5125d52 156 controller.adc1_raw = ADC1->DR;
benkatz 20:bf9ea5125d52 157 //toggle = 0;
benkatz 20:bf9ea5125d52 158 commutate();
benkatz 20:bf9ea5125d52 159
benkatz 2:8724412ad628 160 }
benkatz 2:8724412ad628 161 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 162 }
benkatz 0:4e1c4df6aabd 163
benkatz 20:bf9ea5125d52 164
benkatz 0:4e1c4df6aabd 165
benkatz 0:4e1c4df6aabd 166 int main() {
benkatz 20:bf9ea5125d52 167
benkatz 20:bf9ea5125d52 168 controller.v_bus = V_BUS;
benkatz 9:d7eb815cb057 169 spi.ZeroPosition();
benkatz 20:bf9ea5125d52 170 Init_All_HW(&gpio);
benkatz 20:bf9ea5125d52 171
benkatz 9:d7eb815cb057 172 wait(.1);
benkatz 20:bf9ea5125d52 173 //TIM1->CR1 |= TIM_CR1_UDIS;
benkatz 20:bf9ea5125d52 174 gpio.enable->write(1);
benkatz 20:bf9ea5125d52 175 gpio.pwm_u->write(1.0f); //write duty cycles
benkatz 20:bf9ea5125d52 176 gpio.pwm_v->write(1.0f);
benkatz 20:bf9ea5125d52 177 gpio.pwm_w->write(1.0f);
benkatz 20:bf9ea5125d52 178 zero_current(&controller.adc1_offset, &controller.adc2_offset);
benkatz 20:bf9ea5125d52 179 reset_foc(&controller);
benkatz 20:bf9ea5125d52 180 TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
benkatz 20:bf9ea5125d52 181 gpio.enable->write(1);
benkatz 20:bf9ea5125d52 182 //gpio.pwm_u->write(1.0f - .05f); //write duty cycles
benkatz 20:bf9ea5125d52 183 //gpio.pwm_v->write(1.0f - .05f);
benkatz 20:bf9ea5125d52 184 //gpio.pwm_w->write(1.0f - .1f);
benkatz 20:bf9ea5125d52 185
benkatz 20:bf9ea5125d52 186 wait(.1);
benkatz 9:d7eb815cb057 187 NVIC_SetPriority(TIM5_IRQn, 2);
benkatz 20:bf9ea5125d52 188 //loop.attach(&commutate, .000025);
benkatz 18:f1d56f4acb39 189 can.frequency(1000000); // set bit rate to 1Mbps
benkatz 18:f1d56f4acb39 190 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 18:f1d56f4acb39 191 can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
benkatz 18:f1d56f4acb39 192
benkatz 14:80ce59119d93 193 pc.baud(921600);
benkatz 20:bf9ea5125d52 194 wait(.01);
benkatz 20:bf9ea5125d52 195 pc.printf("HobbyKing Cheetah v1.1\n\r");
benkatz 20:bf9ea5125d52 196 pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
benkatz 20:bf9ea5125d52 197 wait(.01);
benkatz 20:bf9ea5125d52 198
benkatz 20:bf9ea5125d52 199
benkatz 20:bf9ea5125d52 200 controller.i_d_ref = 0;
benkatz 20:bf9ea5125d52 201 controller.i_q_ref = 0;
benkatz 0:4e1c4df6aabd 202 while(1) {
benkatz 11:c83b18d41e54 203
benkatz 0:4e1c4df6aabd 204 }
benkatz 0:4e1c4df6aabd 205 }