Martin Gurtner
Fork of the Ben Katz's motor controller firmware.
Diff: main.cpp
- Revision:
- 44:8040fa2fcb0d
- Parent:
- 43:dfb72608639c
- Child:
- 45:26801179208e
diff -r dfb72608639c -r 8040fa2fcb0d main.cpp
--- a/main.cpp Fri May 25 16:10:46 2018 +0000
+++ b/main.cpp Mon Jun 11 00:04:06 2018 +0000
@@ -9,7 +9,7 @@
#define SETUP_MODE 4
#define ENCODER_MODE 5
-#define VERSION_NUM "1.5"
+#define VERSION_NUM "1.6"
float __float_reg[64]; // Floats stored in flash
@@ -30,7 +30,7 @@
#include "user_config.h"
#include "PreferenceWriter.h"
#include "CAN_com.h"
-
+#include "DRV.h"
PreferenceWriter prefs(6);
@@ -46,6 +46,10 @@
CANMessage txMsg;
+SPI drv_spi(PA_7, PA_6, PA_5);
+DigitalOut drv_cs(PA_4);
+//DigitalOut drv_en_gate(PA_11);
+DRV832x drv(&drv_spi, &drv_cs);
PositionSensorAM5147 spi(16384, 0.0, NPP);
@@ -81,34 +85,50 @@
}
void enter_menu_state(void){
+ drv.disable_gd();
printf("\n\r\n\r\n\r");
printf(" Commands:\n\r");
+ wait_us(10);
printf(" m - Motor Mode\n\r");
+ wait_us(10);
printf(" c - Calibrate Encoder\n\r");
+ wait_us(10);
printf(" s - Setup\n\r");
+ wait_us(10);
printf(" e - Display Encoder\n\r");
+ wait_us(10);
printf(" z - Set Zero Position\n\r");
+ wait_us(10);
printf(" esc - Exit to Menu\n\r");
+ wait_us(10);
state_change = 0;
- gpio.enable->write(0);
+ //gpio.enable->write(0);
gpio.led->write(0);
}
void enter_setup_state(void){
printf("\n\r\n\r Configuration Options \n\r\n\n");
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
+ 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("\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;
}
void enter_torque_mode(void){
+ drv.enable_gd();
controller.ovp_flag = 0;
- gpio.enable->write(1); // Enable gate drive
reset_foc(&controller); // Tesets integrators, and other control loop parameters
wait(.001);
controller.i_d_ref = 0;
@@ -119,14 +139,14 @@
}
void calibrate(void){
- gpio.enable->write(1); // Enable gate drive
+ drv.enable_gd();
gpio.led->write(1); // Turn on status LED
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);
- gpio.enable->write(0); // Turn off gate drive
printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
+ drv.disable_gd();
state_change = 0;
}
@@ -193,16 +213,20 @@
controller.kd = 0;
controller.t_ff = 0;
}
- commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
+ //commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
+ TIM1->CCR3 = (PWM_ARR)*(0.5f); // Write duty cycles
+ TIM1->CCR2 = (PWM_ARR)*(0.5f);
+ TIM1->CCR1 = (PWM_ARR)*(0.5f);
controller.timeout += 1;
/*
count++;
if(count == 4000){
- printf("%.4f\n\r", controller.dtheta_mech);
+ //printf("%.4f\n\r", controller.dtheta_mech);
+
count = 0;
}
- */
+ */
}
@@ -331,14 +355,29 @@
controller.v_bus = V_BUS;
controller.mode = 0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
+ wait(.1);
+
+ gpio.enable->write(1);
+ wait_us(100);
+ drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
+ wait_us(100);
+ drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, 0x3);
+ wait_us(100);
+
+ zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
+
+
+
wait(.1);
+ /*
gpio.enable->write(1);
TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles
TIM1->CCR2 = 0x708*(1.0f);
TIM1->CCR1 = 0x708*(1.0f);
- zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
gpio.enable->write(0);
+ */
+
reset_foc(&controller); // Reset current controller
TIM1->CR1 ^= TIM_CR1_UDIS;
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
@@ -348,7 +387,6 @@
NVIC_SetPriority(CAN1_RX0_IRQn, 3);
can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
- //can.filter(CAN_ID, 0xF, CANStandard, 0);
txMsg.id = CAN_MASTER;
txMsg.len = 6;
@@ -374,13 +412,36 @@
printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
printf(" Output Zero Position: %.4f\n\r", M_OFFSET);
printf(" CAN ID: %d\n\r", CAN_ID);
-
+
+
+
+
+ printf(" %d\n\r", drv.read_register(DCR));
+ wait_us(100);
+ printf(" %d\n\r", drv.read_register(CSACR));
+
+ drv.disable_gd();
+
pc.attach(&serial_interrupt); // attach serial interrupt
state_change = 1;
+
-
+ int counter = 0;
while(1) {
-
+counter++;
+ if(counter>40000)
+ {
+ //gpio.enable->write(1);
+ //wait_us(100);
+ //printf(" %d\n\r", drv.read_register(DCR));
+ //wait_us(100);
+ //printf(" %d\n\r", drv.read_register(CSACR));
+ drv.print_faults();
+ //printf("%d\n\r", drv.read_register(DCR));
+ counter = 0;
+ //gpio.enable->write(0);
+ }
+ wait_us(25);
}
}