ROBOSTEP_5期
BLDC motor driver
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 11:c83b18d41e54
- Parent:
- 10:370851e6e132
- Child:
- 12:c473a25f54f7
--- a/main.cpp Thu May 12 05:02:52 2016 +0000
+++ b/main.cpp Sun May 22 00:14:59 2016 +0000
@@ -8,120 +8,86 @@
#include "TorqueController.h"
#include "ImpedanceController.h"
-///SPI Input Stuff
-//DigitalIn cselect(PB_12);
-//InterruptIn select(PB_12);
-//DigitalIn mosi(PB_15);
-//SPISlave input(PB_15, PB_14, PB_13, PB_12);
+using namespace FastMath;
+using namespace Transforms;
int id[3] = {0};
float cmd_float[3] = {0.0f};
int raw[3] = {0};
-float val_max[3] = {18.0f, 1.0f, 0.1f};
+float val_max[3] = {18.0f, 1.0f, 0.1f}; //max angle in radians, stiffness in N-m/rad, damping in N-m*s/rad
int buff[8];
Serial pc(PA_2, PA_3);
-//PositionSensorEncoder encoder(8192,4.0f);
-//Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
-Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005); //hall motter
-//Inverter inverter(PA_10, PA_9, PA_8, PB_7, 0.01007080078, 0.00005); //test motter
-//PositionSensorSPI spi(2048, 2.75f, 7); ///1 I really need an eeprom or something to store this....
+Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005); //hall motor
+PositionSensorSPI spi(2048, 2.75f, 7); ///1 I really need an eeprom or something to store this....
//PositionSensorSPI spi(2048, 1.34f, 7); ///2
-PositionSensorSPI spi(2048, 3.0, 21);
int motorID = 40; ///1
//int motorID = 50; ///2
-//PositionSensorEncoder encoder(1024, 0, 7);
-PositionSensorEncoder encoder(1024, 0, 21);
+PositionSensorEncoder encoder(1024, 0, 7);
-CurrentRegulator foc(&inverter, &spi, .005, .5); //hall sensor
+
+CurrentRegulator foc(&inverter, &spi, .005, .5);
TorqueController torqueController(.031f, &foc);
ImpedanceController impedanceController(&torqueController, &spi, &encoder);
-//CurrentRegulator foc(&inverter, &encoder, .005, .5); //test motter
-//SVPWM svpwm(&inverter, 2.0f);
-
Ticker testing;
-//Timer t;
-/*
-float v_d = 0;
-float v_q = .1;
-float v_alpha = 0;
-float v_beta = 0;
-float v_a = 0;
-float v_b = 0;
-float v_c = 0;
-*/
-float ref = 0.0;
-int count = 0;
-//SPI spi(PB_15, PB_14, PB_13);
-//GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
-
-//DigitalOut chipselect(PB_12);
-
-using namespace FastMath;
-using namespace Transforms;
// Current Sampling IRQ
-/*
-extern "C" void TIM2_IRQHandler(void) {
- // flash on update event
- if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
- inverter.SampleCurrent();
- }
- TIM2->SR = 0x0; // reset the status register
-}
-*/
-
-
-
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
- // toggle on update event
if (TIM1->SR & TIM_SR_UIF ) {
inverter.SampleCurrent();
- //wait(.00002);
//foc.Commutate(); ///Putting the loop here doesn't work for some reason. Need to figure out why
}
TIM1->SR = 0x0; // reset the status register
- //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
}
-
+// HobbyKing-style startup tone. Just because.
void hk_start(void){
+ float dtc = .1;
inverter.SetDTC(0, 0, 0);
inverter.EnableInverter();
- for(int i = 0; i<120; i++){
- torqueController.SetTorque(.4);
- wait(0.000956);
- torqueController.SetTorque(-.4);
- wait(0.000956);
+ for(int i = 0; i<200; i++){
+ //torqueController.SetTorque(.4);
+ inverter.SetDTC(dtc, 0, 0);
+ wait(0.00047778308);
+ //torqueController.SetTorque(-.4);
+ inverter.SetDTC(0, dtc, 0);
+ wait(0.00047778308);
}
- for(int i = 0; i<120; i++){
- torqueController.SetTorque(.4);
- wait(0.0008513);
- torqueController.SetTorque(-.4);
- wait(0.0008513);
+ for(int i = 0; i<200; i++){
+ //torqueController.SetTorque(.4);
+ inverter.SetDTC(dtc, 0, 0);
+ wait(0.00042565508);
+ //torqueController.SetTorque(-.4);
+ inverter.SetDTC(0, dtc, 0);
+ wait(0.00042565508);
}
- for(int i = 0; i<120; i++){
- torqueController.SetTorque(.4);
- wait(0.00075843);
- torqueController.SetTorque(-.4);
- wait(0.00075843);
+ for(int i = 0; i<200; i++){
+ //torqueController.SetTorque(.4);
+ inverter.SetDTC(dtc, 0, 0);
+ wait(0.00037921593);
+ //torqueController.SetTorque(-.4);
+ inverter.SetDTC(0, dtc, 0);
+ wait(0.00037921593);
}
- torqueController.SetTorque(0);
- wait(.4);
+ inverter.SetDTC(0, 0, 0);
+ wait(1);
for (int j = 0; j<3; j++){
- for(int i = 0; i<120; i++){
- torqueController.SetTorque(.4);
- wait(0.000956);
- torqueController.SetTorque(-.4);
- wait(0.000956);
+ for(int i = 0; i<240; i++){
+ //torqueController.SetTorque(.4);
+ inverter.SetDTC(dtc, 0, 0);
+ wait(0.00047778308);
+ //torqueController.SetTorque(-.4);
+ inverter.SetDTC(0, dtc, 0);
+ wait(0.00047778308);
}
torqueController.SetTorque(0);
+ inverter.SetDTC(0, 0, 0);
wait(.2);
}
@@ -129,7 +95,7 @@
}
-/*
+/* //sinusoidal voltage-mode control, for debugging.
void voltage_foc(void){
float theta = encoder.GetElecPosition();
InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
@@ -138,37 +104,8 @@
//output.write(theta/6.28318530718f);
}
*/
-/*
-void read(void){
- int startByte;
- if(input.receive()){
- //startByte = input.read();
- //if(startByte == 65535){
- //startByte = input.read();
- //wait(.000005);
- raw[0] = input.read();
- raw[1] = input.read();
- raw[2] = input.read();
- id[0] = raw[0]>>14;
- id[1] = raw[1]>>14;
- id[2] = raw[2]>>14;
- printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
- for(int i = 0; i<3; i++){
- cmd_float[id[i]] = (val_max[id[i]])*(float)(raw[i] - (id[i]<<14))/16383.0f;
- }
- // }
- // else{
- // input.read();
- // input.read();
- // input.read();
- // }
- //printf("%d %d %d \n\r", raw[0], raw[1], raw[2]);
- }
-
- }
-
-*/
+// For decoding serial commands.
void serialInterrupt(void){
//wait(.001);
int i = 0;
@@ -203,26 +140,15 @@
//pc.printf("%d\n", cmd);
//pc.printf("%d, %d, %d, %d, %d, %d, %d, %d\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6], buff[7]);
}
-
+
void Loop(void){
- //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
- impedanceController.SetImpedance(-.04, -0.00, 0);
-
- count = count+1;
-
- if(count > 1000){
- //ref= -1*ref;
- //printf("%f %f %f \n\r", cmd_float[0], cmd_float[1], cmd_float[2]);
- //float e = spi.GetElecPosition();
- //printf("%f\n\r", e);
- count = 0;
- }
-
-
+ impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+ //impedanceController.SetImpedance(-.04, 0, 0);
//torqueController.SetTorque(0);
//foc.Commutate();
//voltage_foc();
+
}
void PrintStuff(void){
@@ -235,11 +161,9 @@
//printf("%f %f %f %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
//printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
}
-
-
-
/*
+ ////Throw some sines on the phases. useful to make sure the hardware works.
void gen_sine(void){
float f = 1.0f;
float time = t.read();
@@ -251,34 +175,18 @@
*/
int main() {
- //mosi.mode(PullDown);
- //cselect.mode(PullUp);
inverter.DisableInverter();
spi.ZeroPosition();
- //input.format(16, 0);
- //input.frequency(100000);
- //select.fall(&read);
-
-
- //NVIC_SetPriority(EXTI15_10_IRQn, 1);
wait(.1);
inverter.SetDTC(0.2, 0.2, 0.2);
inverter.EnableInverter();
- hk_start();
- //foc.Reset();
+ //hk_start();
+ foc.Reset();
testing.attach(&Loop, .0001);
NVIC_SetPriority(TIM5_IRQn, 2);
pc.baud(115200);
- //pc.attach(&serialInterrupt);
- //printf("hello\n\r");
- //testing.attach(&gen_sine, .01);
- //testing.attach(&PrintStuff, .1);
- //inverter.SetDTC(.05, 0, 0);
- //inverter.DisableInverter();
- //foc.Commutate();
- wait(.5);
+ wait(.1);
while(1) {
- //printf("%f\n\r", encoder.GetElecPosition());
- //wait(.1);
+
}
}