Ben Katz
FOC Implementation for putting multirotor motors in robots
Diff: main.cpp
- Revision:
- 9:d7eb815cb057
- Parent:
- 8:10ae7bc88d6e
- Child:
- 10:370851e6e132
--- a/main.cpp Wed Apr 13 04:09:56 2016 +0000
+++ b/main.cpp Tue May 10 01:15:57 2016 +0000
@@ -6,19 +6,40 @@
#include "Transforms.h"
#include "CurrentRegulator.h"
#include "TorqueController.h"
-//#include "ImpedanceController.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);
+
+int id[3] = {0};
+float cmd_float[3] = {0.0f};
+int raw[3] = {0};
+float val_max[3] = {18.0f, 1.0f, 0.1f};
+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.01007080078, 0.00005); //hall motter
+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);
+PositionSensorSPI spi(2048, 2.75f); ///1 I really need an eeprom or something to store this....
+//PositionSensorSPI spi(2048, 1.34f); ///2
+//PositionSensorSPI spi(2048, 1);
+int motorID = 40; ///1
+//int motorID = 50; ///2
+
+PositionSensorEncoder encoder(1024, 0);
CurrentRegulator foc(&inverter, &spi, .005, .5); //hall sensor
-TorqueController torqueController(.061f, &foc);
+TorqueController torqueController(.031f, &foc);
+ImpedanceController impedanceController(&torqueController, &spi, &encoder);
+
//CurrentRegulator foc(&inverter, &encoder, .005, .5); //test motter
+//SVPWM svpwm(&inverter, 2.0f);
-//SVPWM svpwm(&inverter, 2.0f);
Ticker testing;
//Timer t;
@@ -31,6 +52,8 @@
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
@@ -59,7 +82,7 @@
if (TIM1->SR & TIM_SR_UIF ) {
inverter.SampleCurrent();
//wait(.00002);
- //foc.Commutate();
+ //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
@@ -76,19 +99,102 @@
//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]);
+ }
+
+ }
+
+*/
+
+ void serialInterrupt(void){
+ //wait(.001);
+ int i = 0;
+ while(pc.readable()){
+ buff[i] = pc.getc();
+ wait(.0001);
+ i++;
+
+ }
+ int val = (buff[4]<<8) + buff[5];
+ int checksum = buff[2]^buff[3]^buff[4]^buff[5];
+ int validStart = (buff[0] == 255 && buff[1] == 255 && buff[2]==motorID && checksum==buff[6]);
+
+ if(validStart){
+
+ switch(buff[3]){
+ case 10:
+ cmd_float[1] = (float)val*val_max[1]/65278.0f;
+ break;
+ case 20:
+ cmd_float[2] = (float)val*val_max[2]/65278.0f;
+ break;
+ case 30:
+ cmd_float[0] = (float)val*val_max[0]/65278.0f;
+ break;
+ }
+ }
+
+
+ //pc.printf("%d %d %d %d %d %d %d \n", start1, start2, id, cmd, byte1, byte2, byte3);
+ //pc.printf("%f, %f, %f\n", cmd_float[0], cmd_float[1], cmd_float[2]);
+ //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){
- foc.Commutate();
+
+ impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+ //impedanceController.SetImpedance(-.4, -0.006, 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;
+ }
+
+
+ //torqueController.SetTorque(0);
+ //foc.Commutate();
//voltage_foc();
}
void PrintStuff(void){
- //float velocity = encoder.GetMechVelocity();
+ //float v = encoder.GetMechVelocity();
//float position = encoder.GetElecPosition();
//float position = encoder.GetMechPosition();
- float m = spi.GetMechPosition();
+ //float m = spi.GetMechPosition();
float e = spi.GetElecPosition();
- //printf("%f, %f;\n\r", position, velocity);
- printf("Elec: %f \n\r", m);
+ printf("%f\n\r", e);
+ //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]);
}
@@ -106,16 +212,31 @@
*/
int main() {
- //t.start();
- wait(1);
+ //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();
+ foc.Reset();
testing.attach(&Loop, .0001);
-
- NVIC_SetPriority(TIM5_IRQn, 1);
+ 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.EnableInverter();
+ //inverter.DisableInverter();
//foc.Commutate();
+ wait(.5);
while(1) {
//printf("%f\n\r", encoder.GetElecPosition());
//wait(.1);