Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

main.cpp@7:dc5f27756e02, 2016-03-29 (annotated)

Committer:
benkatz
Date:
Tue Mar 29 01:05:46 2016 +0000
Revision:
7:dc5f27756e02
Parent:
6:4ee1cdc43aa8
Child:
8:10ae7bc88d6e
propper svm;

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 7:dc5f27756e02 8 //#include "TorqueController.h"
benkatz 7:dc5f27756e02 9 //#include "ImpedanceController.h"
benkatz 0:4e1c4df6aabd 10 PositionSensorEncoder encoder(8192,0);
benkatz 2:8724412ad628 11 //Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
benkatz 7:dc5f27756e02 12 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.01007080078, 0.00005);
benkatz 0:4e1c4df6aabd 13 CurrentRegulator foc(&inverter, &encoder, .005, .5);
benkatz 7:dc5f27756e02 14 SVPWM svpwm(&inverter, 2.0f);
benkatz 7:dc5f27756e02 15 Ticker testing;
benkatz 7:dc5f27756e02 16 //Timer t;
benkatz 0:4e1c4df6aabd 17
benkatz 7:dc5f27756e02 18 float v_d = 0;
benkatz 7:dc5f27756e02 19 float v_q = .1;
benkatz 7:dc5f27756e02 20 float v_alpha = 0;
benkatz 7:dc5f27756e02 21 float v_beta = 0;
benkatz 7:dc5f27756e02 22 float v_a = 0;
benkatz 7:dc5f27756e02 23 float v_b = 0;
benkatz 7:dc5f27756e02 24 float v_c = 0;
benkatz 4:c023f7b6f462 25
benkatz 4:c023f7b6f462 26 //SPI spi(PB_15, PB_14, PB_13);
benkatz 4:c023f7b6f462 27 //GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
benkatz 4:c023f7b6f462 28
benkatz 4:c023f7b6f462 29 //DigitalOut chipselect(PB_12);
benkatz 4:c023f7b6f462 30
benkatz 0:4e1c4df6aabd 31 using namespace FastMath;
benkatz 0:4e1c4df6aabd 32 using namespace Transforms;
benkatz 0:4e1c4df6aabd 33
benkatz 0:4e1c4df6aabd 34
benkatz 1:b8bceb4daed5 35 // Current Sampling IRQ
benkatz 2:8724412ad628 36 /*
benkatz 0:4e1c4df6aabd 37 extern "C" void TIM2_IRQHandler(void) {
benkatz 0:4e1c4df6aabd 38 // flash on update event
benkatz 0:4e1c4df6aabd 39 if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
benkatz 0:4e1c4df6aabd 40 inverter.SampleCurrent();
benkatz 0:4e1c4df6aabd 41 }
benkatz 0:4e1c4df6aabd 42 TIM2->SR = 0x0; // reset the status register
benkatz 0:4e1c4df6aabd 43 }
benkatz 2:8724412ad628 44 */
benkatz 2:8724412ad628 45 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 46 // toggle on update event
benkatz 2:8724412ad628 47 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 48 inverter.SampleCurrent();
benkatz 7:dc5f27756e02 49 //wait(.00002);
benkatz 7:dc5f27756e02 50 //foc.Commutate();
benkatz 2:8724412ad628 51 }
benkatz 2:8724412ad628 52 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 53 //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
benkatz 2:8724412ad628 54 }
benkatz 0:4e1c4df6aabd 55
benkatz 7:dc5f27756e02 56 void voltage_foc(void){
benkatz 7:dc5f27756e02 57 float theta = encoder.GetElecPosition();
benkatz 7:dc5f27756e02 58 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 7:dc5f27756e02 59 InvClarke(v_alpha, v_beta, &v_a, &v_b, &v_c);
benkatz 7:dc5f27756e02 60 svpwm.Update_DTC(v_a, v_b, v_c);
benkatz 7:dc5f27756e02 61 //output.write(theta/6.28318530718f);
benkatz 7:dc5f27756e02 62 }
benkatz 7:dc5f27756e02 63
benkatz 0:4e1c4df6aabd 64 void Loop(void){
benkatz 0:4e1c4df6aabd 65 foc.Commutate();
benkatz 7:dc5f27756e02 66 //voltage_foc();
benkatz 0:4e1c4df6aabd 67 }
benkatz 3:6a0015d88d06 68
benkatz 3:6a0015d88d06 69 void PrintStuff(void){
benkatz 3:6a0015d88d06 70 float velocity = encoder.GetMechVelocity();
benkatz 3:6a0015d88d06 71 float position = encoder.GetMechPosition();
benkatz 7:dc5f27756e02 72 //printf("%f, %f;\n\r", position, velocity);
benkatz 7:dc5f27756e02 73 printf("%f %d %d\n\r", position, TIM3->CNT-0x8000, -2*((int)((TIM3->CR1)>>4)&1)+1);
benkatz 3:6a0015d88d06 74 }
benkatz 7:dc5f27756e02 75
benkatz 7:dc5f27756e02 76
benkatz 1:b8bceb4daed5 77
benkatz 7:dc5f27756e02 78
benkatz 7:dc5f27756e02 79 /*
benkatz 7:dc5f27756e02 80 void gen_sine(void){
benkatz 7:dc5f27756e02 81 float f = 1.0f;
benkatz 7:dc5f27756e02 82 float time = t.read();
benkatz 7:dc5f27756e02 83 float a = .45f*sin(6.28318530718f*f*time) + .5f;
benkatz 7:dc5f27756e02 84 float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
benkatz 7:dc5f27756e02 85 float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
benkatz 7:dc5f27756e02 86 inverter.SetDTC(a, b, c);
benkatz 7:dc5f27756e02 87 }
benkatz 7:dc5f27756e02 88 */
benkatz 0:4e1c4df6aabd 89
benkatz 0:4e1c4df6aabd 90 int main() {
benkatz 7:dc5f27756e02 91 //t.start();
benkatz 0:4e1c4df6aabd 92 wait(1);
benkatz 0:4e1c4df6aabd 93 testing.attach(&Loop, .0001);
benkatz 7:dc5f27756e02 94 NVIC_SetPriority(TIM5_IRQn, 1);
benkatz 7:dc5f27756e02 95 //testing.attach(&gen_sine, .01);
benkatz 7:dc5f27756e02 96 //testing.attach(&PrintStuff, .1);
benkatz 3:6a0015d88d06 97 //inverter.SetDTC(.1, 0, 0);
benkatz 7:dc5f27756e02 98 inverter.EnableInverter();
benkatz 7:dc5f27756e02 99 //foc.Commutate();
benkatz 0:4e1c4df6aabd 100 while(1) {
benkatz 3:6a0015d88d06 101 //printf("%f\n\r", encoder.GetElecPosition());
benkatz 3:6a0015d88d06 102 //wait(.1);
benkatz 0:4e1c4df6aabd 103 }
benkatz 0:4e1c4df6aabd 104 }