Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

main.cpp@12:c473a25f54f7, 2016-05-22 (annotated)

Committer:
benkatz
Date:
Sun May 22 03:38:19 2016 +0000
Revision:
12:c473a25f54f7
Parent:
11:c83b18d41e54
Child:
14:80ce59119d93
experimental feed-forward decoupling;

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 11:c83b18d41e54 22 PositionSensorSPI spi(2048, 2.75f, 7); ///1 I really need an eeprom or something to store this....
benkatz 10:370851e6e132 23 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
benkatz 9:d7eb815cb057 24 int motorID = 40; ///1
benkatz 9:d7eb815cb057 25 //int motorID = 50; ///2
benkatz 9:d7eb815cb057 26
benkatz 11:c83b18d41e54 27 PositionSensorEncoder encoder(1024, 0, 7);
benkatz 10:370851e6e132 28
benkatz 8:10ae7bc88d6e 29
benkatz 11:c83b18d41e54 30
benkatz 12:c473a25f54f7 31 CurrentRegulator foc(&inverter, &spi, &encoder, 0.000033, .005, .5);
benkatz 9:d7eb815cb057 32 TorqueController torqueController(.031f, &foc);
benkatz 9:d7eb815cb057 33 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
benkatz 9:d7eb815cb057 34
benkatz 7:dc5f27756e02 35 Ticker testing;
benkatz 0:4e1c4df6aabd 36
benkatz 4:c023f7b6f462 37
benkatz 0:4e1c4df6aabd 38
benkatz 0:4e1c4df6aabd 39
benkatz 1:b8bceb4daed5 40 // Current Sampling IRQ
benkatz 2:8724412ad628 41 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 42 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 43 inverter.SampleCurrent();
benkatz 9:d7eb815cb057 44 //foc.Commutate(); ///Putting the loop here doesn't work for some reason. Need to figure out why
benkatz 2:8724412ad628 45 }
benkatz 2:8724412ad628 46 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 47 }
benkatz 0:4e1c4df6aabd 48
benkatz 11:c83b18d41e54 49 // HobbyKing-style startup tone. Just because.
benkatz 10:370851e6e132 50 void hk_start(void){
benkatz 11:c83b18d41e54 51 float dtc = .1;
benkatz 10:370851e6e132 52 inverter.SetDTC(0, 0, 0);
benkatz 10:370851e6e132 53 inverter.EnableInverter();
benkatz 11:c83b18d41e54 54 for(int i = 0; i<200; i++){
benkatz 11:c83b18d41e54 55 //torqueController.SetTorque(.4);
benkatz 11:c83b18d41e54 56 inverter.SetDTC(dtc, 0, 0);
benkatz 11:c83b18d41e54 57 wait(0.00047778308);
benkatz 11:c83b18d41e54 58 //torqueController.SetTorque(-.4);
benkatz 11:c83b18d41e54 59 inverter.SetDTC(0, dtc, 0);
benkatz 11:c83b18d41e54 60 wait(0.00047778308);
benkatz 10:370851e6e132 61 }
benkatz 11:c83b18d41e54 62 for(int i = 0; i<200; i++){
benkatz 11:c83b18d41e54 63 //torqueController.SetTorque(.4);
benkatz 11:c83b18d41e54 64 inverter.SetDTC(dtc, 0, 0);
benkatz 11:c83b18d41e54 65 wait(0.00042565508);
benkatz 11:c83b18d41e54 66 //torqueController.SetTorque(-.4);
benkatz 11:c83b18d41e54 67 inverter.SetDTC(0, dtc, 0);
benkatz 11:c83b18d41e54 68 wait(0.00042565508);
benkatz 10:370851e6e132 69 }
benkatz 11:c83b18d41e54 70 for(int i = 0; i<200; i++){
benkatz 11:c83b18d41e54 71 //torqueController.SetTorque(.4);
benkatz 11:c83b18d41e54 72 inverter.SetDTC(dtc, 0, 0);
benkatz 11:c83b18d41e54 73 wait(0.00037921593);
benkatz 11:c83b18d41e54 74 //torqueController.SetTorque(-.4);
benkatz 11:c83b18d41e54 75 inverter.SetDTC(0, dtc, 0);
benkatz 11:c83b18d41e54 76 wait(0.00037921593);
benkatz 10:370851e6e132 77 }
benkatz 11:c83b18d41e54 78 inverter.SetDTC(0, 0, 0);
benkatz 11:c83b18d41e54 79 wait(1);
benkatz 10:370851e6e132 80 for (int j = 0; j<3; j++){
benkatz 11:c83b18d41e54 81 for(int i = 0; i<240; i++){
benkatz 11:c83b18d41e54 82 //torqueController.SetTorque(.4);
benkatz 11:c83b18d41e54 83 inverter.SetDTC(dtc, 0, 0);
benkatz 11:c83b18d41e54 84 wait(0.00047778308);
benkatz 11:c83b18d41e54 85 //torqueController.SetTorque(-.4);
benkatz 11:c83b18d41e54 86 inverter.SetDTC(0, dtc, 0);
benkatz 11:c83b18d41e54 87 wait(0.00047778308);
benkatz 10:370851e6e132 88 }
benkatz 10:370851e6e132 89 torqueController.SetTorque(0);
benkatz 11:c83b18d41e54 90 inverter.SetDTC(0, 0, 0);
benkatz 10:370851e6e132 91 wait(.2);
benkatz 10:370851e6e132 92
benkatz 10:370851e6e132 93 }
benkatz 10:370851e6e132 94
benkatz 10:370851e6e132 95 }
benkatz 10:370851e6e132 96
benkatz 8:10ae7bc88d6e 97
benkatz 11:c83b18d41e54 98 /* //sinusoidal voltage-mode control, for debugging.
benkatz 7:dc5f27756e02 99 void voltage_foc(void){
benkatz 7:dc5f27756e02 100 float theta = encoder.GetElecPosition();
benkatz 7:dc5f27756e02 101 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 7:dc5f27756e02 102 InvClarke(v_alpha, v_beta, &v_a, &v_b, &v_c);
benkatz 7:dc5f27756e02 103 svpwm.Update_DTC(v_a, v_b, v_c);
benkatz 7:dc5f27756e02 104 //output.write(theta/6.28318530718f);
benkatz 7:dc5f27756e02 105 }
benkatz 8:10ae7bc88d6e 106 */
benkatz 9:d7eb815cb057 107
benkatz 11:c83b18d41e54 108 // For decoding serial commands.
benkatz 9:d7eb815cb057 109 void serialInterrupt(void){
benkatz 9:d7eb815cb057 110 //wait(.001);
benkatz 9:d7eb815cb057 111 int i = 0;
benkatz 9:d7eb815cb057 112 while(pc.readable()){
benkatz 9:d7eb815cb057 113 buff[i] = pc.getc();
benkatz 9:d7eb815cb057 114 wait(.0001);
benkatz 9:d7eb815cb057 115 i++;
benkatz 9:d7eb815cb057 116
benkatz 9:d7eb815cb057 117 }
benkatz 9:d7eb815cb057 118 int val = (buff[4]<<8) + buff[5];
benkatz 9:d7eb815cb057 119 int checksum = buff[2]^buff[3]^buff[4]^buff[5];
benkatz 9:d7eb815cb057 120 int validStart = (buff[0] == 255 && buff[1] == 255 && buff[2]==motorID && checksum==buff[6]);
benkatz 9:d7eb815cb057 121
benkatz 9:d7eb815cb057 122 if(validStart){
benkatz 9:d7eb815cb057 123
benkatz 9:d7eb815cb057 124 switch(buff[3]){
benkatz 9:d7eb815cb057 125 case 10:
benkatz 9:d7eb815cb057 126 cmd_float[1] = (float)val*val_max[1]/65278.0f;
benkatz 9:d7eb815cb057 127 break;
benkatz 9:d7eb815cb057 128 case 20:
benkatz 9:d7eb815cb057 129 cmd_float[2] = (float)val*val_max[2]/65278.0f;
benkatz 9:d7eb815cb057 130 break;
benkatz 9:d7eb815cb057 131 case 30:
benkatz 9:d7eb815cb057 132 cmd_float[0] = (float)val*val_max[0]/65278.0f;
benkatz 9:d7eb815cb057 133 break;
benkatz 9:d7eb815cb057 134 }
benkatz 9:d7eb815cb057 135 }
benkatz 9:d7eb815cb057 136
benkatz 9:d7eb815cb057 137
benkatz 9:d7eb815cb057 138 //pc.printf("%d %d %d %d %d %d %d \n", start1, start2, id, cmd, byte1, byte2, byte3);
benkatz 9:d7eb815cb057 139 //pc.printf("%f, %f, %f\n", cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 140 //pc.printf("%d\n", cmd);
benkatz 9:d7eb815cb057 141 //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]);
benkatz 9:d7eb815cb057 142 }
benkatz 11:c83b18d41e54 143
benkatz 0:4e1c4df6aabd 144 void Loop(void){
benkatz 9:d7eb815cb057 145
benkatz 11:c83b18d41e54 146 impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
benkatz 11:c83b18d41e54 147 //impedanceController.SetImpedance(-.04, 0, 0);
benkatz 9:d7eb815cb057 148 //torqueController.SetTorque(0);
benkatz 9:d7eb815cb057 149 //foc.Commutate();
benkatz 7:dc5f27756e02 150 //voltage_foc();
benkatz 11:c83b18d41e54 151
benkatz 0:4e1c4df6aabd 152 }
benkatz 3:6a0015d88d06 153
benkatz 3:6a0015d88d06 154 void PrintStuff(void){
benkatz 9:d7eb815cb057 155 //float v = encoder.GetMechVelocity();
benkatz 8:10ae7bc88d6e 156 //float position = encoder.GetElecPosition();
benkatz 8:10ae7bc88d6e 157 //float position = encoder.GetMechPosition();
benkatz 9:d7eb815cb057 158 //float m = spi.GetMechPosition();
benkatz 10:370851e6e132 159 //float e = spi.GetElecPosition();
benkatz 10:370851e6e132 160 //printf("%f\n\r", e);
benkatz 9:d7eb815cb057 161 //printf("%f %f %f %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
benkatz 9:d7eb815cb057 162 //printf("%d %d %d\n\r", raw[0], raw[1], raw[2]);
benkatz 3:6a0015d88d06 163 }
benkatz 7:dc5f27756e02 164
benkatz 7:dc5f27756e02 165 /*
benkatz 11:c83b18d41e54 166 ////Throw some sines on the phases. useful to make sure the hardware works.
benkatz 7:dc5f27756e02 167 void gen_sine(void){
benkatz 7:dc5f27756e02 168 float f = 1.0f;
benkatz 7:dc5f27756e02 169 float time = t.read();
benkatz 7:dc5f27756e02 170 float a = .45f*sin(6.28318530718f*f*time) + .5f;
benkatz 7:dc5f27756e02 171 float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
benkatz 7:dc5f27756e02 172 float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
benkatz 7:dc5f27756e02 173 inverter.SetDTC(a, b, c);
benkatz 7:dc5f27756e02 174 }
benkatz 7:dc5f27756e02 175 */
benkatz 0:4e1c4df6aabd 176
benkatz 0:4e1c4df6aabd 177 int main() {
benkatz 9:d7eb815cb057 178 inverter.DisableInverter();
benkatz 9:d7eb815cb057 179 spi.ZeroPosition();
benkatz 9:d7eb815cb057 180 wait(.1);
benkatz 9:d7eb815cb057 181 inverter.SetDTC(0.2, 0.2, 0.2);
benkatz 9:d7eb815cb057 182 inverter.EnableInverter();
benkatz 11:c83b18d41e54 183 //hk_start();
benkatz 11:c83b18d41e54 184 foc.Reset();
benkatz 0:4e1c4df6aabd 185 testing.attach(&Loop, .0001);
benkatz 9:d7eb815cb057 186 NVIC_SetPriority(TIM5_IRQn, 2);
benkatz 9:d7eb815cb057 187 pc.baud(115200);
benkatz 11:c83b18d41e54 188 wait(.1);
benkatz 0:4e1c4df6aabd 189 while(1) {
benkatz 11:c83b18d41e54 190
benkatz 0:4e1c4df6aabd 191 }
benkatz 0:4e1c4df6aabd 192 }