Austin Brown
Mike Etek Controller
Diff: main.cpp
- Revision:
- 1:94193b31f0ee
- Parent:
- 0:9edd6ec0f56a
--- a/main.cpp Sat May 20 21:42:20 2017 +0000
+++ b/main.cpp Sat Apr 06 02:16:12 2019 +0000
@@ -1,241 +1,236 @@
#include "mbed.h"
-#include "PositionSensor.h"
-#include "structs.h"
+
+#include "fw_config.h"
+#include "math_ops.h"
+#include "Inverter.h"
+#include "ServoInput.h"
+
+
+/*
+
+
+PA2 goes to servo input
+
+PA3-7, PA0-1 goes to analog input (on of those)
-#include "Inverter.h"
-//#define PI 3.14159f
+use two of either PA8, 9, 10,
+slaved with PA7, PB0, PB1
+
+PA7 and PA8 for high and low sides
+
+PB6, 7 used for serial
+PA13, 14, reset used for programming
-#include "foc.h"
+some other random pin used for LED
+some other random pin used for clockpin, for debugging
+
+
+======
+pins finally used by mike:
+PA2: RC PWM
+PA5: clockpin
+PA8: gate drive
+*/
-DigitalOut clockpin(PC_12);
-//AnalogIn pot_in(PC_3);
+
+
+DigitalOut clockpin(PA_5);
+DigitalOut led1(PB_1);
+DigitalIn gpio1(PA_6);
+
+//DigitalIn gpio2(PA_2);
// controller modes
#define REST_MODE 0
-#define OLSINE 1
-#define ONEUPTWODOWN 2
-#define VMODESINE 3
-//#define BEN_CALIBRATION_MODE 1
-#define TORQUE_MODE 4
-//#define PD_MODE 5
-//#define SETUP_MODE 6
-//#define ENCODER_MODE 7
-
+#define VOLTAGE_MODE 1
+#define CURRENT_MODE 2
+
-GPIOStruct gpio;
-ControllerStruct controller;
+volatile int count = 0;
+volatile int main_int_count = 0;
+volatile unsigned int main_int_clock = 0;
+
+int controller_state = 0;
-//float fake_theta = 0.0;
-float theta1 = 0.0f;
-//float theta_offset = 4.708f;
-//float theta_offset = 3.38f;
-//float theta_offset = 4.97f;
-float theta_offset = 4.8f;
+Serial pc(PB_6, PB_7);
+Inverter inverter;
+ServoTimer servoinp;
-int bing1 = 0;
-int bing2 = 0;
-int bing3 = 0;
+
+float a1 = 0.0f;
+float a2 = 0.0f;
+
-int var1 = 0;
-int var2 = 0;
+float current = 0.0f;
+float current_raw = 0.0f;
+float servo_cmd = 0.0f;
-float a = 0.0f;
-float b = 0.0f;
-float c = 0.0f;
-volatile int count = 0;
-
-int controller_state = ONEUPTWODOWN;//VMODESINE;
+int servo_low = 890;
+int servo_high = 1800;
+int servo_range = servo_high - servo_low;
-Serial pc(PA_2, PA_3);
-PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS);
// Main 20khz loop Interrupt ///
-/// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers ///
-extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
- //clockpin = 1;
+
+extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
+
+ main_int_count++;
+ main_int_clock++;
+
+ //
+ //current_raw = float(ADC1->DR) - float(inverter.adc1_offset);
+ current_raw = float(ADC1->DR) - 2000;
+ //current_raw = float(ADC1->DR));
+ //current = current_raw*0.1f; //depends on shunts
+
+ inverter.adc2_raw = ADC2->DR;
+ inverter.adc1_raw = ADC1->DR;
+ clockpin = 1;
+ servoinp.update_servo_input();
- ///Sample current always ///
- ADC1->CR2 |= 0x40000000; //Begin sample and conversion
- //volatile int delay;
- //for (delay = 0; delay < 55; delay++);
- controller.adc2_raw = ADC2->DR;
- controller.adc1_raw = ADC1->DR;
-
-
-
/// Check state machine state, and run the appropriate function ///
- //printf("%d\n\r", state);
switch(controller_state){
case REST_MODE: //nothing
- if(count > 1000){
+ TIM1->CCR1 = 6400;
+ count++;
+ if(count > 20000){
count = 0;
- printf("Rest Mode");
- printf("\n\r");
+ led1 = !led1;
}
- break;
+ servo_cmd = servoinp.get_servo_input();
+
+ break;
+
- case OLSINE: // open loop sines
+ case VOLTAGE_MODE:
+
+ servo_cmd = servoinp.get_servo_input();
- //theta1+= 0.1f*pot_in;
- theta1+= 0.001f;
- if (theta1 > 2*PI) { theta1-=(2*PI); }
-
- //trigger clock pin sychronous with electrical revolutions
- if (theta1 > PI) { clockpin = 1; }
- else {clockpin = 0;}
+ TIM1->CCR1 = 6400 - constrain((servo_cmd-880)*8.0f , 0, 6150); //0 to 96% duty cycle
- a = cosf(theta1)/2 + 0.5f;
- b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
- c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
-
- TIM1->CCR1 = 0x1194*(a);
- TIM1->CCR2 = 0x1194*(b);
- TIM1->CCR3 = 0x1194*(c);
- break;
+ if(count > 10000){ count = 0; }
+
+ break;
+
+ case CURRENT_MODE:
- case ONEUPTWODOWN: // one up, two down
-
- a = 0.8f;
- b = 0.2f;
- c = 0.2f;
+ servo_cmd = servoinp.get_servo_input();
+ /*
+ float current_setpoint = (servo_cmd-880)/100; //0 to 10 amps
+ if (current > current_setpoint) {
+ //set bridge state to LOW
+ TIM1->CCR1 = 6100;
+ }
+ else {
+ TIM1->CCR1 = 300;
+ }*/
+
- count++;
- if(count > 500){
- count = 0;
- //printf("%f ",encoder.GetElecPosition());
+ // servo inputs go from 880 to 1700
+
+ // the constant in the next line is set 1800/servo_range
+ //float current_setpoint_raw = (servo_cmd-servo_low)*2.25f; //scales 800 range to 1800
+ float current_setpoint_raw = constrain((servo_cmd-servo_low)*2.0f , 0, 800); //scales 800 range to 1800
+ if (current_raw > current_setpoint_raw) {
+ //set bridge state to LOW
+ TIM1->CCR1 = 6400;
+ }
+ else {
+ TIM1->CCR1 = 250;
}
- TIM1->CCR1 = 0x1194*(a);
- TIM1->CCR2 = 0x1194*(b);
- TIM1->CCR3 = 0x1194*(c);
- break;
-
- case VMODESINE: // position mode sines
- theta1 = encoder.GetElecPosition() + theta_offset;
-
- if (theta1 > 2*PI) {
- theta1-=(2*PI);
+ if ((servo_cmd < servo_low) & (current_raw < 200) & (current_raw > -200)) {
+ TIM1->CCR1 = 6400;
}
-
- //trigger clock pin sychronous with electrical revolutions
- if (encoder.GetElecPosition() > PI) { clockpin = 1; }
- else {clockpin = 0;}
-
- a = cosf(theta1)/2 + 0.5f;
- b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
- c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
-
- TIM1->CCR1 = 0x1194*(a);
- TIM1->CCR2 = 0x1194*(b);
- TIM1->CCR3 = 0x1194*(c);
- //remove this code later
- //print Q and D values
- count++;
- // Run current loop
- //spi.Sample(); // Sample position sensor
- if(count > 1000){
- count = 0;
- //printf("%f ",controller.i_q);
- //printf("%f ",controller.i_d);
- //printf("\n\r");
- }
+ if(count > 10000){ count = 0; }
- break;
-
- case TORQUE_MODE:
-
- // Run torque control
- count++;
- controller.theta_elec = encoder.GetElecPosition() + theta_offset;
- commutate(&controller, &gpio, controller.theta_elec); // Run current loop
- //spi.Sample(); // Sample position sensor
- if(count > 1000){
- count = 0;
- //readCAN();
- //controller.i_q_ref = ((float)(canCmd-1000))/100;
- controller.i_q_ref = 2;
- //pc.printf("%f\n\r ", controller.theta_elec);
+ break;
- //printf("%i ",controller.adc1_raw);
- //printf("%i \n\r ",controller.adc2_raw);
- }
- }
-
-
- ///*
-
- controller.theta_elec = encoder.GetElecPosition() + theta_offset;
- commutate(&controller, &gpio, controller.theta_elec);
-
-
-
- //*/
+
+ }
}
+ //b = TIM1->CNT;
TIM1->SR = 0x0;
- //clockpin = 0; // reset the status register
+ clockpin = 0; // reset the status register
}
int main() {
- //float meas;
- pc.baud(115200);
- printf("\nStarting Hardware\n");
- Init_All_HW(&gpio);
+ wait_ms(200);
+ pc.baud(256000);
+ printf("\rStarting Hardware\n");
+ gpio1.mode(PullUp);
+
- zero_current(&controller.adc1_offset, &controller.adc2_offset); // Setup PWM, ADC, GPIO
+ inverter.Init(); // Setup PWM, ADC
+ wait(0.1);
+ controller_state = REST_MODE;
+ wait(0.1);
+ inverter.zero_current();
wait(0.1);
+ pc.printf("ADCs zeroed at ");
+ pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset);
+ wait(0.1);
+ inverter.ADCsync();
+
+
+ //controller_state = VOLTAGE_MODE;
+ controller_state = CURRENT_MODE;
+
while(1) {
- //printf("AnalogIn example\n");
- //printf("%f ",a);
- //printf("\n");
- wait(0.1);
-
- if (0==0) {
- //printf("%i ", TIM3->CNT);
-
- printf("%f ",encoder.GetElecPosition());
- //printf("%f ",encoder.GetMechPosition());
-
- printf("%f ",theta_offset);
+
+ wait_us(2);
- printf("%i ",controller.adc1_raw);
- printf("%i ",controller.adc2_raw);
-
- printf("%i ",controller.adc1_offset);
- printf("%i ",controller.adc2_offset);
+ while (1==1) {
+
+ wait_us(30);
+
+ pc.printf("%f, ", servo_cmd);
- printf("%f ",controller.i_q);
- printf("%f ",controller.i_d);
+ pc.printf("%f, ", current_raw);
- printf("%f ",controller.i_a);
- printf("%f ",controller.i_b);
-
- //printf("%f ",controller.theta_elec);
+ pc.printf("%f, ", float(inverter.adc1_offset));
+ pc.printf("%f, ", a1);
+ pc.printf("%f, ", a2);
-
- printf("\n\r");
-
+ //printf("%i, ", b3);
+ //printf("%i, ", b4);
+
+ //pc.printf("%i, ", TIM15->CNT);
+ //pc.printf("%x ", GPIOA->AFR[0]);
+
+ pc.printf("%i, ", ADC1->DR);
+ pc.printf("%i, ", ADC2->DR);
+ //printf(" %i, %i %i", a1, a2, a3);
+ //if (gpio2) {pc.printf("low ");}
+ pc.printf("\r");
+
+
}
-
-
-
-
+
}
}
+
+
+
+
+
+
+