Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
Diff: main.cpp
- Revision:
- 58:32e8927fe39f
- Parent:
- 57:0795d2add37e
- Child:
- 60:0c8caaa91182
diff -r 0795d2add37e -r 32e8927fe39f main.cpp --- a/main.cpp Thu Nov 07 18:14:51 2019 +0000 +++ b/main.cpp Tue Feb 04 14:19:58 2020 +0000 @@ -53,7 +53,8 @@ //DigitalOut drv_en_gate(PA_11); DRV832x drv(&drv_spi, &drv_cs); -PositionSensoriCMU spi(16777216, 0.0, NPP); // Read 24 bits off the sensor +//PositionSensoriCMU spi(16777216, 0.0, NPP); // Read 24 bits off the sensor +PositionSensoriCMU spi(524288, 0.0, NPP); // Read 24 bits off the sensor, only 19bits are encoder data volatile int count = 0; volatile int state = REST_MODE; @@ -62,6 +63,7 @@ void onMsgReceived() { //msgAvailable = true; //printf("%d\n\r", rxMsg.id); + gpio.led->write(1); can.read(rxMsg); if((rxMsg.id == CAN_ID)){ controller.timeout = 0; @@ -80,9 +82,11 @@ else if(state == MOTOR_MODE){ unpack_cmd(rxMsg, &controller); } - pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); + pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); // multiply theta, dtheta_mech by GR here? + //pack_reply(&txMsg, 3.14159f, 6.28318f, 1.00f); // test values can.write(txMsg); } + gpio.led->write(0); } @@ -158,9 +162,9 @@ wait(.05); R_NOMINAL = 0; state = INIT_TEMP_MODE; - //printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); + printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); //drv.disable_gd(); - //state_change = 0; + state_change = 0; } @@ -191,8 +195,8 @@ controller.adc1_raw = ADC1->DR; controller.adc3_raw = ADC3->DR; controller.theta_elec = spi.GetElecPosition(); - controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); - controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); + controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); // should these be divided by gear ratio??? + controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); // mech pos isn't pre-multiplied by GR in position sensor function... controller.dtheta_elec = spi.GetElecVelocity(); controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement