Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

main.cpp@14:80ce59119d93, 2016-10-31 (annotated)

Committer:
benkatz
Date:
Mon Oct 31 16:48:16 2016 +0000
Revision:
14:80ce59119d93
Parent:
12:c473a25f54f7 
Misc. changes.  Finally fixed transforms (turns out B and C current measurements were accidentally swapped)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4e1c4df6aabd 1 #include "mbed.h"
benkatz 0:4e1c4df6aabd 2 #include "PositionSensor.h"
benkatz 0:4e1c4df6aabd 3 #include "Inverter.h"
benkatz 0:4e1c4df6aabd 4 #include "SVM.h"
benkatz 0:4e1c4df6aabd 5 #include "FastMath.h"
benkatz 0:4e1c4df6aabd 6 #include "Transforms.h"
benkatz 0:4e1c4df6aabd 7 #include "CurrentRegulator.h"
benkatz 8:10ae7bc88d6e 8 #include "TorqueController.h"
benkatz 9:d7eb815cb057 9 #include "ImpedanceController.h"
benkatz 9:d7eb815cb057 10
benkatz 11:c83b18d41e54 11 using namespace FastMath;
benkatz 11:c83b18d41e54 12 using namespace Transforms;
benkatz 9:d7eb815cb057 13
benkatz 9:d7eb815cb057 14 int id[3] = {0};
benkatz 9:d7eb815cb057 15 float cmd_float[3] = {0.0f};
benkatz 9:d7eb815cb057 16 int raw[3] = {0};
benkatz 11:c83b18d41e54 17 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
benkatz 9:d7eb815cb057 18 int buff[8];
benkatz 9:d7eb815cb057 19 Serial pc(PA_2, PA_3);
benkatz 8:10ae7bc88d6e 20
benkatz 11:c83b18d41e54 21 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005); //hall motor
benkatz 14:80ce59119d93 22 PositionSensorAM5147 spi(16384, 2.7f, 7); ///1 I really need an eeprom or something to store this....
benkatz 10:370851e6e132 23 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
benkatz 14:80ce59119d93 24
benkatz 9:d7eb815cb057 25
benkatz 14:80ce59119d93 26 PositionSensorEncoder encoder(4096, 0, 7);
benkatz 10:370851e6e132 27
benkatz 8:10ae7bc88d6e 28
benkatz 11:c83b18d41e54 29
benkatz 12:c473a25f54f7 30 CurrentRegulator foc(&inverter, &spi, &encoder, 0.000033, .005, .5);
benkatz 9:d7eb815cb057 31 TorqueController torqueController(.031f, &foc);
benkatz 9:d7eb815cb057 32 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
benkatz 9:d7eb815cb057 33
benkatz 7:dc5f27756e02 34 Ticker testing;
benkatz 0:4e1c4df6aabd 35
benkatz 4:c023f7b6f462 36
benkatz 0:4e1c4df6aabd 37
benkatz 0:4e1c4df6aabd 38
benkatz 1:b8bceb4daed5 39 // Current Sampling IRQ
benkatz 2:8724412ad628 40 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 41 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 42 inverter.SampleCurrent();
benkatz 9:d7eb815cb057 43 //foc.Commutate(); ///Putting the loop here doesn't work for some reason. Need to figure out why
benkatz 2:8724412ad628 44 }
benkatz 2:8724412ad628 45 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 46 }
benkatz 0:4e1c4df6aabd 47
benkatz 14:80ce59119d93 48 int count = 0;
benkatz 14:80ce59119d93 49 void Loop(void){
benkatz 14:80ce59119d93 50 count++;
benkatz 14:80ce59119d93 51 //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
benkatz 14:80ce59119d93 52 impedanceController.SetImpedance(.1, -0.01, 0);
benkatz 10:370851e6e132 53
benkatz 14:80ce59119d93 54 //torqueController.SetTorque(-.03);
benkatz 9:d7eb815cb057 55 //foc.Commutate();
benkatz 7:dc5f27756e02 56 //voltage_foc();
benkatz 14:80ce59119d93 57 if(count>2000){
benkatz 14:80ce59119d93 58 //float e = spi.GetElecPosition();
benkatz 14:80ce59119d93 59 //float v = encoder.GetMechVelocity();
benkatz 14:80ce59119d93 60 //printf("%f\n\r", v);
benkatz 14:80ce59119d93 61 //printf("IA: %f IB: %f IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
benkatz 14:80ce59119d93 62 count = 0;
benkatz 14:80ce59119d93 63 }
benkatz 11:c83b18d41e54 64
benkatz 0:4e1c4df6aabd 65 }
benkatz 3:6a0015d88d06 66
benkatz 3:6a0015d88d06 67 void PrintStuff(void){
benkatz 14:80ce59119d93 68 //inverter.SetDTC(0.03, 0.0, 0.0);
benkatz 14:80ce59119d93 69
benkatz 9:d7eb815cb057 70 //float v = encoder.GetMechVelocity();
benkatz 8:10ae7bc88d6e 71 //float position = encoder.GetElecPosition();
benkatz 14:80ce59119d93 72 int position = spi.GetRawPosition();
benkatz 9:d7eb815cb057 73 //float m = spi.GetMechPosition();
benkatz 14:80ce59119d93 74 float e = spi.GetElecPosition();
benkatz 14:80ce59119d93 75 foc.Commutate();
benkatz 14:80ce59119d93 76 float q = foc.GetQ();
benkatz 14:80ce59119d93 77 printf("position: %d angle: %f q current: %f\n\r", position, e, q);
benkatz 14:80ce59119d93 78 //inverter.getCurrent()
benkatz 9:d7eb815cb057 79 //printf("%f %f %f %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 80 //printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
benkatz 14:80ce59119d93 81 //printf("IA: %f IB: %f IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
benkatz 3:6a0015d88d06 82 }
benkatz 7:dc5f27756e02 83
benkatz 7:dc5f27756e02 84 /*
benkatz 11:c83b18d41e54 85 ////Throw some sines on the phases. useful to make sure the hardware works.
benkatz 7:dc5f27756e02 86 void gen_sine(void){
benkatz 7:dc5f27756e02 87 float f = 1.0f;
benkatz 7:dc5f27756e02 88 float time = t.read();
benkatz 7:dc5f27756e02 89 float a = .45f*sin(6.28318530718f*f*time) + .5f;
benkatz 7:dc5f27756e02 90 float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
benkatz 7:dc5f27756e02 91 float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
benkatz 7:dc5f27756e02 92 inverter.SetDTC(a, b, c);
benkatz 7:dc5f27756e02 93 }
benkatz 7:dc5f27756e02 94 */
benkatz 0:4e1c4df6aabd 95
benkatz 0:4e1c4df6aabd 96 int main() {
benkatz 9:d7eb815cb057 97 inverter.DisableInverter();
benkatz 9:d7eb815cb057 98 spi.ZeroPosition();
benkatz 9:d7eb815cb057 99 wait(.1);
benkatz 14:80ce59119d93 100 inverter.SetDTC(0.03, 0.0, 0.0);
benkatz 9:d7eb815cb057 101 inverter.EnableInverter();
benkatz 11:c83b18d41e54 102 foc.Reset();
benkatz 14:80ce59119d93 103 testing.attach(&Loop, .000025);
benkatz 14:80ce59119d93 104 //testing.attach(&PrintStuff, .05);
benkatz 9:d7eb815cb057 105 NVIC_SetPriority(TIM5_IRQn, 2);
benkatz 14:80ce59119d93 106 pc.baud(921600);
benkatz 14:80ce59119d93 107 pc.printf("HobbyKing Cheeta v1.1\n\r");
benkatz 11:c83b18d41e54 108 wait(.1);
benkatz 0:4e1c4df6aabd 109 while(1) {
benkatz 11:c83b18d41e54 110
benkatz 0:4e1c4df6aabd 111 }
benkatz 0:4e1c4df6aabd 112 }