Austin Brown
Slurp
Diff: main.cpp
- Revision:
- 0:9edd6ec0f56a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat May 20 21:42:20 2017 +0000
@@ -0,0 +1,241 @@
+#include "mbed.h"
+#include "PositionSensor.h"
+#include "structs.h"
+
+#include "Inverter.h"
+//#define PI 3.14159f
+
+#include "foc.h"
+
+
+
+
+DigitalOut clockpin(PC_12);
+//AnalogIn pot_in(PC_3);
+
+
+// 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
+
+
+GPIOStruct gpio;
+ControllerStruct controller;
+
+
+//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;
+
+int bing1 = 0;
+int bing2 = 0;
+int bing3 = 0;
+
+int var1 = 0;
+int var2 = 0;
+
+float a = 0.0f;
+float b = 0.0f;
+float c = 0.0f;
+
+volatile int count = 0;
+
+int controller_state = ONEUPTWODOWN;//VMODESINE;
+
+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;
+
+ if (TIM1->SR & TIM_SR_UIF ) {
+
+
+ ///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){
+ count = 0;
+ printf("Rest Mode");
+ printf("\n\r");
+ }
+ break;
+
+ case OLSINE: // open loop sines
+
+ //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;}
+
+ 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;
+
+ case ONEUPTWODOWN: // one up, two down
+
+ a = 0.8f;
+ b = 0.2f;
+ c = 0.2f;
+
+ count++;
+ if(count > 500){
+ count = 0;
+ //printf("%f ",encoder.GetElecPosition());
+ }
+
+ 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);
+ }
+
+ //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");
+ }
+
+ 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);
+
+ //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);
+
+
+
+ //*/
+
+ }
+ TIM1->SR = 0x0;
+ //clockpin = 0; // reset the status register
+}
+
+int main() {
+ //float meas;
+ pc.baud(115200);
+ printf("\nStarting Hardware\n");
+ Init_All_HW(&gpio);
+
+ zero_current(&controller.adc1_offset, &controller.adc2_offset); // Setup PWM, ADC, GPIO
+ wait(0.1);
+
+
+ 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);
+
+ printf("%i ",controller.adc1_raw);
+ printf("%i ",controller.adc2_raw);
+
+ printf("%i ",controller.adc1_offset);
+ printf("%i ",controller.adc2_offset);
+
+ printf("%f ",controller.i_q);
+ printf("%f ",controller.i_d);
+
+ printf("%f ",controller.i_a);
+ printf("%f ",controller.i_b);
+
+ //printf("%f ",controller.theta_elec);
+
+
+
+ printf("\n\r");
+
+ }
+
+
+
+
+ }
+}