hongwei liu
test
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 23:2adf23ee0305
- Parent:
- 22:60276ba87ac6
- Child:
- 24:58c2d7571207
--- a/main.cpp Fri Mar 31 18:24:46 2017 +0000
+++ b/main.cpp Wed Apr 05 20:54:16 2017 +0000
@@ -4,17 +4,21 @@
/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
+#define REST_MODE 0
+#define CALIBRATION_MODE 1
+#define TORQUE_MODE 2
+#define PD_MODE 3
+#define SETUP_MODE 4
+#define ENCODER_MODE 5
const unsigned int BOARDNUM = 0x2;
-
//const unsigned int a_id =
-
const unsigned int TX_ID = 0x0100;
-
const unsigned int cmd_ID = (BOARDNUM<<8) + 0x7;
-
+float __float_reg[64];
+int __int_reg[256];
#include "CANnucleo.h"
#include "mbed.h"
@@ -23,10 +27,19 @@
#include "foc.h"
#include "calibration.h"
#include "hw_setup.h"
-#include "math_ops.h"
+#include "math_ops.h"
#include "current_controller_config.h"
#include "hw_config.h"
#include "motor_config.h"
+#include "stm32f4xx_flash.h"
+#include "FlashWriter.h"
+#include "user_config.h"
+#include "PreferenceWriter.h"
+//#include "state_machine.h"
+
+
+
+PreferenceWriter prefs(6);
GPIOStruct gpio;
ControllerStruct controller;
@@ -35,7 +48,7 @@
-CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
+CANnucleo::CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
CANnucleo::CANMessage rxMsg;
CANnucleo::CANMessage txMsg;
int ledState;
@@ -68,160 +81,218 @@
void readCAN(void){
if(msgAvailable) {
- msgAvailable = false; // reset flag for next use
- can.read(rxMsg); // read message into Rx message storage
+ msgAvailable = false; // reset flag for next use
+ can.read(rxMsg); // read message into Rx message storage
// Filtering performed by software:
- if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
- rxMsg >> canCmd;
- } // extract first data item
+ if(rxMsg.id == cmd_ID) { // See comments in CAN.cpp for filtering performed by hardware
+ rxMsg >> canCmd; // extract first data item
+ }
}
- }
+ }
void cancontroller(void){
//printf("%d\n\r", canCmd);
readCAN();
//sendCMD(TX_ID, canCmd);
-
- //sendCMD(TX_ID+b_ID, b1);
- //sendCMD(TX_ID+c_ID, c1);
+
}
Serial pc(PA_2, PA_3);
-PositionSensorAM5147 spi(16384, -0.4658, NPP); ///1 I really need an eeprom or something to store this....
+PositionSensorAM5147 spi(16384, 0.0, NPP);
PositionSensorEncoder encoder(4096, 0, 21);
-int count = 0;
-int mode = 0;
+volatile int count = 0;
+volatile int state = REST_MODE;
+volatile int state_change;
-
-float velocity_estimate(void){
- velocity.vel_2 = encoder.GetMechVelocity();
- velocity.vel_1 = spi.GetMechVelocity();
+void enter_menu_state(void){
+ printf("\n\r\n\r\n\r");
+ printf(" Commands:\n\r");
+ printf(" t - Torque Mode\n\r");
+ printf(" p - PD Mode\n\r");
+ printf(" c - Calibrate Encoder\n\r");
+ printf(" s - Setup\n\r");
+ printf(" e - Display Encoder\n\r");
+ printf(" esc - Exit to Menu\n\r");
+ state_change = 0;
+ }
- velocity.ts = .01f;
- velocity.vel_1_est = velocity.ts*velocity.vel_1 + (1-velocity.ts)*velocity.vel_1_est; //LPF
- velocity.vel_2_est = (1-velocity.ts)*(velocity.vel_2_est + velocity.vel_2 - velocity.vel_2_old); //HPF
- velocity.est = velocity.vel_1_est + velocity.vel_2_est;
+void enter_torque_mode(void){
+ controller.mode = 2;
+ controller.i_d_ref = 0;
+ controller.i_q_ref = 0;
+ reset_foc(&controller); //resets integrators, and other control loop parameters
+ gpio.enable->write(1);
+ GPIOC->ODR ^= (1 << 5); //turn on status LED
+ }
- velocity.vel_1_old = velocity.vel_1;
- velocity.vel_2_old = velocity.vel_2;
- return velocity.est;
+void calibrate(void){
+ gpio.enable->write(1);
+ GPIOC->ODR ^= (1 << 5);
+ order_phases(&spi, &gpio, &controller, &prefs);
+ calibrate(&spi, &gpio, &controller, &prefs);
+ GPIOC->ODR ^= (1 << 5);
+ wait(.2);
+ gpio.enable->write(0);
+ printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
+ state_change = 0;
+
+ }
+
+void print_encoder(void){
+ spi.Sample();
+ wait(.001);
+ printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+ wait(.05);
}
-// Current Sampling Interrupt
+/// Current Sampling Interrupt ///
+/// This runs at 40 kHz, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
//toggle = 1;
- count++;
- ADC1->CR2 |= 0x40000000; //Begin sample and conversion
+
+ ///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;
+ ///
- if(controller.mode == 2){
- controller.adc2_raw = ADC2->DR;
- controller.adc1_raw = ADC1->DR;
-
- //toggle = 0;
+ /// Check state machine state, and run the appropriate function ///
+ //printf("%d\n\r", state);
+ switch(state){
+ case REST_MODE: // Do nothing until
+ if(state_change){
+ enter_menu_state();
+ }
+ break;
- spi.Sample();
- controller.theta_elec = spi.GetElecPosition();
- commutate(&controller, &gpio, controller.theta_elec);
- }
-
-
- //controller.dtheta_mech = spi.GetMechVelocity();
- //controller.dtheta_elec = encoder.GetElecVelocity();
- //ontroller.dtheta_mech = encoder.GetMechVelocity();
- //controller.i_q_ref = 2.0f*controller.dtheta_mech;
-
-
- //float v1 = encoder.GetMechVelocity();
- //float v2 = spi.GetMechVelocity();
-
-
- if(count > 100){
- count = 0;
- //for (int i = 1; i<16; i++){
- //pc.printf("%d\n\r ", spi.GetRawPosition());
- // }
- //pc.printf("\n\r");
- //pc.printf("%.4f %.4f %.4f\n\r",velocity.vel_1 ,velocity.vel_2, velocity.est );
-
- }
-
+ case CALIBRATION_MODE: // Run encoder calibration procedure
+ if(state_change){
+ calibrate();
+ }
+ break;
+
+ case TORQUE_MODE: // Run torque control
+ count++;
+ controller.theta_elec = spi.GetElecPosition();
+ commutate(&controller, &gpio, controller.theta_elec); // Run current loop
+ spi.Sample(); // Sample position sensor
+ if(count > 100){
+ count = 0;
+ readCAN();
+ controller.i_q_ref = ((float)(canCmd-1000))/100;
+ //pc.printf("%f\n\r ", controller.theta_elec);
+ }
+ break;
+
+ case PD_MODE:
+ break;
+ case SETUP_MODE:
+ if(state_change){
+ printf("\n\r Configuration Menu \n\r\n\n");
+ state_change = 0;
+ }
+ break;
+ case ENCODER_MODE:
+ print_encoder();
+ break;
+ }
+
+
}
- TIM1->SR = 0x0; // reset the status register
+ TIM1->SR = 0x0; // reset the status register
}
-
-void enter_torque_mode(void){
- controller.mode = 2;
- TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupts
- controller.i_d_ref = 0;
- controller.i_q_ref = 1;
- reset_foc(&controller); //resets integrators, and other control loop parameters
- gpio.enable->write(1);
- GPIOC->ODR ^= (1 << 5); //turn on LED
+/// Manage state machine with commands from serial terminal or configurator gui ///
+void serial_interrupt(void){
+ while(pc.readable()){
+ char c = pc.getc();
+ if(c == 27){
+ state = REST_MODE;
+ state_change = 1;
+ }
+ else if(state == REST_MODE){
+ switch (c){
+ case 'c':
+ state = CALIBRATION_MODE;
+ state_change = 1;
+ break;
+ case 't':
+ state = TORQUE_MODE;
+ state_change = 1;
+ break;
+ case 'e':
+ state = ENCODER_MODE;
+ state_change = 1;
+ break;
+ case 's':
+ state = SETUP_MODE;
+ state_change = 1;
+ break;
+
+ }
+ }
}
-
-void enter_calibration_mode(void){
- controller.mode = 1;
- TIM1->CR1 ^= TIM_CR1_UDIS;
- gpio.enable->write(1);
- GPIOC->ODR ^= (1 << 5);
- //calibrate_encoder(&spi);
- order_phases(&spi, &gpio);
- calibrate(&spi, &gpio);
- GPIOC->ODR ^= (1 << 5);
- wait(.2);
- gpio.enable->write(0);
- TIM1->CR1 ^= TIM_CR1_UDIS;
- controller.mode = 0;
}
-
int main() {
+
+
controller.v_bus = V_BUS;
controller.mode = 0;
//spi.ZeroPosition();
- Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
+ Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
wait(.1);
//TIM1->CR1 |= TIM_CR1_UDIS;
- gpio.enable->write(1);
- gpio.pwm_u->write(1.0f); //write duty cycles
+ gpio.enable->write(1); // Enable gate drive
+ gpio.pwm_u->write(1.0f); // Write duty cycles
gpio.pwm_v->write(1.0f);
gpio.pwm_w->write(1.0f);
- zero_current(&controller.adc1_offset, &controller.adc2_offset); //Measure current sensor zero-offset
+ zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
//gpio.enable->write(0);
- reset_foc(&controller);
+ reset_foc(&controller); // Reset current controller
- //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
- //gpio.enable->write(1);
- //gpio.pwm_u->write(1.0f - .05f); //write duty cycles
- //gpio.pwm_v->write(1.0f - .05f);
- //gpio.pwm_w->write(1.0f - .1f);
+ TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt
wait(.1);
- NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority
+ NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority
- can.frequency(1000000); // set bit rate to 1Mbps
- can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
+ can.frequency(1000000); // set bit rate to 1Mbps
+ can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
- pc.baud(115200);
+
+ prefs.load();
+ spi.SetElecOffset(E_OFFSET);
+ int lut[128] = {0};
+ memcpy(&lut, &ENCODER_LUT, sizeof(lut));
+ spi.WriteLUT(lut);
+
+ pc.baud(115200); // set serial baud rate
wait(.01);
- pc.printf("HobbyKing Cheetah v1.1\n\r");
- pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset);
+ pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
wait(.01);
-
+ printf("\n\r Debug Info:\n\r");
+ printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
+ printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
+ printf(" CAN ID: %d\n\r", BOARDNUM);
+
+ pc.attach(&serial_interrupt); // attach serial interrupt
- enter_calibration_mode();
- enter_torque_mode();
+ state_change = 1;
+ //enter_menu_state(); //Print available commands, wait for command
+ //enter_calibration_mode();
+ //wait_us(100);
+
+ //enter_torque_mode();
while(1) {