Fork and fix for mwork
Dependencies: mbed-dev-f303 FastPWM3 millis
main.cpp
- Committer:
- benkatz
- Date:
- 2017-03-02
- Revision:
- 20:bf9ea5125d52
- Parent:
- 19:bd10a04eedc2
- Child:
- 22:60276ba87ac6
File content as of revision 20:bf9ea5125d52:
const unsigned int BOARDNUM = 0x2; //const unsigned int a_id = const unsigned int TX_ID = 0x0100; const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7; #include "CANnucleo.h" #include "mbed.h" #include "PositionSensor.h" #include "structs.h" #include "foc.h" #include "hw_setup.h" #include "math_ops.h" #include "current_controller_config.h" #include "hw_config.h" #include "motor_config.h" GPIOStruct gpio; ControllerStruct controller; COMStruct com; CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name CANnucleo::CANMessage rxMsg; CANnucleo::CANMessage txMsg; int ledState; int counter = 0; int canCmd = 1000; volatile bool msgAvailable = false; DigitalOut toggle(PA_0); Ticker loop; /** * @brief 'CAN receive-complete' interrup handler. * @note Called on arrival of new CAN message. * Keep it as short as possible. * @param * @retval */ void onMsgReceived() { msgAvailable = true; //printf("ping\n\r"); } void sendCMD(int TX_addr, int val){ txMsg.clear(); //clear Tx message storage txMsg.id = TX_addr; txMsg << val; can.write(txMsg); //wait(.1); } void readCAN(void){ if(msgAvailable) { msgAvailable = false; // reset flag for next use can.read(rxMsg); // read message into Rx message storage // Filtering performed by software: if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware rxMsg >> canCmd; } // extract first data item } } void cancontroller(void){ //printf("%d\n\r", canCmd); readCAN(); //sendCMD(TX_ID, canCmd); //sendCMD(TX_ID+b_ID, b1); //sendCMD(TX_ID+c_ID, c1); } Serial pc(PA_2, PA_3); PositionSensorAM5147 spi(16384, 4.7, NPP); ///1 I really need an eeprom or something to store this.... //PositionSensorEncoder encoder(4096, 0, 21); int count = 0; void commutate(void){ count ++; //pc.printf("%f\n\r", controller.theta_elec); //Get rotor angle //spi.GetMechPosition(); controller.i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset); //Calculate phase currents from ADC readings controller.i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset); controller.i_a = -controller.i_b - controller.i_c; dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q); //dq0 transform on currents ///Control Law/// float i_d_error = controller.i_d_ref - controller.i_d; float i_q_error = controller.i_q_ref - controller.i_q; float v_d_ff = controller.i_d_ref*R_TOTAL; //feed-forward voltage float v_q_ff = controller.i_q_ref*R_TOTAL; controller.d_int += i_d_error; controller.q_int += i_q_error; limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q)); //controller.d_int = fminf(fmaxf(controller.d_int, -D_INT_LIM), D_INT_LIM); //controller.q_int = fminf(fmaxf(controller.q_int, -Q_INT_LIM), Q_INT_LIM); controller.v_d = K_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff; controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff; //controller.v_d = 10*v_d_ff; //controller.v_q = 10*v_q_ff; limit_norm(&controller.v_d, &controller.v_q, controller.v_bus); abc(controller.theta_elec, controller.v_d, controller.v_q, &controller.v_u, &controller.v_v, &controller.v_w); //inverse dq0 transform on voltages 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 gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles gpio.pwm_v->write(1.0f-controller.dtc_v); gpio.pwm_w->write(1.0f-controller.dtc_w); controller.theta_elec = spi.GetElecPosition(); //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v); //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w); //gpio.pwm_u->write(1.0f - .1f); //write duty cycles //gpio.pwm_v->write(1.0f - .1f); //gpio.pwm_w->write(1.0f - .15f); if(count >1000){ controller.i_q_ref = -controller.i_q_ref; count = 0; //pc.printf("%f\n\r", controller.theta_elec); //pc.printf("%f %f %f\n\r", controller.i_a, controller.i_b, controller.i_c); //pc.printf("%f %f\n\r", controller.i_d, controller.i_q); //pc.printf("%d %d\n\r", controller.adc1_raw, controller.adc2_raw); } } // Current Sampling IRQ extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { //toggle = 1; ADC1->CR2 |= 0x40000000; //volatile int delay; //for (delay = 0; delay < 55; delay++); controller.adc2_raw = ADC2->DR; controller.adc1_raw = ADC1->DR; //toggle = 0; commutate(); } TIM1->SR = 0x0; // reset the status register } int main() { controller.v_bus = V_BUS; spi.ZeroPosition(); Init_All_HW(&gpio); wait(.1); //TIM1->CR1 |= TIM_CR1_UDIS; gpio.enable->write(1); gpio.pwm_u->write(1.0f); //write duty cycles gpio.pwm_v->write(1.0f); gpio.pwm_w->write(1.0f); zero_current(&controller.adc1_offset, &controller.adc2_offset); reset_foc(&controller); TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt gpio.enable->write(1); //gpio.pwm_u->write(1.0f - .05f); //write duty cycles //gpio.pwm_v->write(1.0f - .05f); //gpio.pwm_w->write(1.0f - .1f); wait(.1); NVIC_SetPriority(TIM5_IRQn, 2); //loop.attach(&commutate, .000025); can.frequency(1000000); // set bit rate to 1Mbps can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler can.filter(0x020 << 25, 0xF0000004, CANAny, 0); pc.baud(921600); wait(.01); pc.printf("HobbyKing Cheetah v1.1\n\r"); pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset); wait(.01); controller.i_d_ref = 0; controller.i_q_ref = 0; while(1) { } }