Ben Katz
Mini Cheetah Actuator Branch Superseded by: https://github.com/bgkatz/motorcontrol
Dependencies: mbed-dev-f303 FastPWM3
Superseded by: https://github.com/bgkatz/motorcontrol
Diff: main.cpp
- Revision:
- 55:c4c9fec8539c
- Parent:
- 53:e85efce8c1eb
- Child:
- 56:fe5056ac6740
--- a/main.cpp Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp Fri Oct 04 14:18:39 2019 +0000
@@ -4,13 +4,14 @@
/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
/// Version for the TI DRV8323 Everything Chip
-#define REST_MODE 0
-#define CALIBRATION_MODE 1
-#define MOTOR_MODE 2
-#define SETUP_MODE 4
-#define ENCODER_MODE 5
+#define REST_MODE 0
+#define CALIBRATION_MODE 1
+#define MOTOR_MODE 2
+#define SETUP_MODE 4
+#define ENCODER_MODE 5
+#define INIT_TEMP_MODE 6
-#define VERSION_NUM "1.9"
+#define VERSION_NUM "1.10"
float __float_reg[64]; // Floats stored in flash
@@ -19,7 +20,7 @@
#include "mbed.h"
#include "PositionSensor.h"
#include "structs.h"
-#include "foc.h"
+#include "foc.h"
#include "calibration.h"
#include "hw_setup.h"
#include "math_ops.h"
@@ -87,6 +88,7 @@
void enter_menu_state(void){
drv.disable_gd();
+ reset_foc(&controller);
//gpio.enable->write(0);
printf("\n\r\n\r\n\r");
printf(" Commands:\n\r");
@@ -124,6 +126,10 @@
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "h", "Temp Cutoff (C) (0 = none)", "0", "150", TEMP_MAX);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "c", "Continuous Current (A)", "0", "40.0", I_MAX_CONT);
+ wait_us(10);
printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
wait_us(10);
state_change = 0;
@@ -149,11 +155,13 @@
order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
gpio.led->write(0);; // Turn off status LED
- wait(.2);
- printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
- drv.disable_gd();
- //gpio.enable->write(0);
- state_change = 0;
+ wait(.05);
+ R_NOMINAL = 0;
+ state = INIT_TEMP_MODE;
+ //printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
+ //drv.disable_gd();
+ //state_change = 0;
+
}
void print_encoder(void){
@@ -164,14 +172,16 @@
/// Current Sampling Interrupt ///
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
+//float testing[1000];
+//float testing2[1000];
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
-
+ //gpio.led->write(1);
///Sample current always ///
ADC1->CR2 |= 0x40000000; // Begin sample and conversion
//volatile int delay;
//for (delay = 0; delay < 55; delay++);
-
+
spi.Sample(DT); // sample position sensor
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
@@ -181,6 +191,9 @@
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
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
+
+
+
///
/// Check state machine state, and run the appropriate function ///
@@ -189,6 +202,7 @@
if(state_change){
enter_menu_state();
}
+ update_observer(&controller, &observer);
break;
case CALIBRATION_MODE: // Run encoder calibration procedure
@@ -196,7 +210,45 @@
calibrate();
}
break;
-
+ case INIT_TEMP_MODE:
+ if(state_change){
+ enter_torque_mode();
+ count = 0;
+ observer.resistance = 0.0f;
+ }
+ controller.i_d_ref = -10.0f;
+ controller.i_q_ref = 0.0f;
+ commutate(&controller, &observer, &gpio, controller.theta_elec);
+
+ if(count > 200)
+ {
+ float r_meas = controller.v_d*(DTC_MAX-DTC_MIN)/(controller.i_d*SQRT3);
+ //testing2[count-100] = controller.i_d;
+ observer.resistance += .001f*r_meas;
+ }
+ if(count > 1200)
+ {
+ count = 0;
+ state = REST_MODE;
+ state_change = 1;
+ gpio.led->write(0);
+ observer.temperature = (double)(T_AMBIENT + ((observer.resistance/R_NOMINAL) - 1.0f)*254.5f);
+ printf("Winding Resistance: %f\n\r", observer.resistance);
+ printf("Winding Temperature: %f\n\r", observer.temperature);
+
+ if(R_NOMINAL==0)
+ {
+ printf("Saving winding resistance\n\r");
+ R_NOMINAL = observer.resistance;
+ if (!prefs.ready()) prefs.open();
+ prefs.flush(); // write offset and lookup table to flash
+ prefs.close();
+ }
+ //for(int i = 0; i<1000; i++){printf("%f \n\r", testing[i]);}
+ }
+
+ count++;
+ break;
case MOTOR_MODE: // Run torque control
if(state_change){
enter_torque_mode();
@@ -221,15 +273,26 @@
controller.kd = 0;
controller.t_ff = 0;
}
-
+
torque_control(&controller);
+ update_observer(&controller, &observer);
+ field_weaken(&controller);
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
-
controller.timeout++;
+
+ if(controller.otw_flag)
+ {
+ state = REST_MODE;
+ state_change = 1;
+ gpio.led->write(0);
+ }
+
count++;
+ }
+
+
+ break;
- }
- break;
case SETUP_MODE:
if(state_change){
enter_setup_state();
@@ -240,6 +303,7 @@
break;
}
}
+ //gpio.led->write(0);
TIM1->SR = 0x0; // reset the status register
}
@@ -258,7 +322,7 @@
state_change = 1;
char_count = 0;
cmd_id = 0;
- gpio.led->write(0);;
+ gpio.led->write(0);;
for(int i = 0; i<8; i++){cmd_val[i] = 0;}
}
if(state == REST_MODE){
@@ -316,6 +380,12 @@
case 't':
CAN_TIMEOUT = atoi(cmd_val);
break;
+ case 'h':
+ TEMP_MAX = fmaxf(fminf(atof(cmd_val), 150.0f), 0.0f);
+ break;
+ case 'c':
+ I_MAX_CONT = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f);
+ break;
default:
printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
break;
@@ -363,46 +433,41 @@
controller.v_bus = V_BUS;
controller.mode = 0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
- wait(.1);
+ wait_us(100);
gpio.enable->write(1);
wait_us(100);
drv.calibrate();
wait_us(100);
- drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
+ drv.write_DCR(0x0, DIS_GDF_DIS, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
+ wait_us(100);
+ drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x1, 0x1, 0x1, SEN_LVL_1_0); // calibrate shunt amplifiers
wait_us(100);
- drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
+ zero_current(&controller.adc1_offset, &controller.adc2_offset);
wait_us(100);
- drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
+ drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x1, 0x0, 0x0, 0x0, SEN_LVL_1_0);
+ wait_us(100);
+ drv.write_OCPCR(TRETRY_50US, DEADTIME_50NS, OCP_NONE, OCP_DEG_8US, VDS_LVL_1_88);
//drv.enable_gd();
- zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
drv.disable_gd();
+ //zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
+ //drv.enable_gd();
- wait(.1);
- /*
- gpio.enable->write(1);
- TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles
- TIM1->CCR2 = 0x708*(1.0f);
- TIM1->CCR1 = 0x708*(1.0f);
- gpio.enable->write(0);
- */
+ wait_us(100);
+
reset_foc(&controller); // Reset current controller
reset_observer(&observer); // Reset observer
- TIM1->CR1 ^= TIM_CR1_UDIS;
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
- wait(.1);
+ wait_us(100);
NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication
-
NVIC_SetPriority(CAN1_RX0_IRQn, 3);
// attach 'CAN receive-complete' interrupt handler
// If preferences haven't been user configured yet, set defaults
prefs.load(); // Read flash
-
- can.filter(CAN_ID , 0xFFF, CANStandard, 0);
-
+ can.filter(CAN_ID , 0xFFF, CANStandard, 0);
txMsg.id = CAN_MASTER;
txMsg.len = 6;
rxMsg.len = 8;
@@ -412,16 +477,20 @@
if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
- if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
+ if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=12;}
if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
- if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;}
+ if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 1000;}
+ if(isnan(R_NOMINAL) || R_NOMINAL==-1){R_NOMINAL = 0.0f;}
+ if(isnan(TEMP_MAX) || TEMP_MAX==-1){TEMP_MAX = 125.0f;}
+ if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;}
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
int lut[128] = {0};
memcpy(&lut, &ENCODER_LUT, sizeof(lut));
spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
init_controller_params(&controller);
+
pc.baud(921600); // set serial baud rate
wait(.01);
@@ -435,33 +504,29 @@
printf(" CAN ID: %d\n\r", CAN_ID);
-
+ TIM1->CR1 ^= TIM_CR1_UDIS;
- //printf(" %d\n\r", drv.read_register(DCR));
- //wait_us(100);
- //printf(" %d\n\r", drv.read_register(CSACR));
- //wait_us(100);
- //printf(" %d\n\r", drv.read_register(OCPCR));
- //drv.disable_gd();
pc.attach(&serial_interrupt); // attach serial interrupt
-
- state_change = 1;
int counter = 0;
while(1) {
- drv.print_faults();
+ //drv.print_faults();
wait(.1);
- //printf("%.4f\n\r", controller.v_bus);
- /*
+ //printf("%.3f %.3f\n\r" , observer.temperature, observer.q_in);
+ if(controller.otw_flag){gpio.led->write(!gpio.led->read());}
+ /*
if(state == MOTOR_MODE)
{
+ if(controller.otw_flag){gpio.led->write(!gpio.led->read());}
+ //printf("%f %f\n\r", controller.dtheta_mech, controller.i_d_ref);
//printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
//printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
- //printf("%.3f\n\r", controller.dtheta_mech);
- wait(.002);
+ //printf("%.3f %.3f %.3f %.3f\n\r", controller.dtheta_elec, observer.resistance, observer.temperature, observer.temp_measured);
+ //printf("%.3f %.3f\n\r" , observer.temperature, observer.temp_measured);
}
+
*/
}