Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

main.cpp@6:4ee1cdc43aa8, 2016-03-12 (annotated)

Committer:
benkatz
Date:
Sat Mar 12 19:55:52 2016 +0000
Revision:
6:4ee1cdc43aa8
Parent:
4:c023f7b6f462
Child:
7:dc5f27756e02
Switched from sinusoidal pwm to 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 0:4e1c4df6aabd 8
benkatz 0:4e1c4df6aabd 9 PositionSensorEncoder encoder(8192,0);
benkatz 2:8724412ad628 10 //Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
benkatz 2:8724412ad628 11 Inverter inverter(PA_8, PA_9, PA_10, PB_7, 0.02014160156, 0.00005);
benkatz 0:4e1c4df6aabd 12 CurrentRegulator foc(&inverter, &encoder, .005, .5);
benkatz 0:4e1c4df6aabd 13
benkatz 0:4e1c4df6aabd 14 Ticker testing;
benkatz 0:4e1c4df6aabd 15
benkatz 2:8724412ad628 16
benkatz 4:c023f7b6f462 17
benkatz 4:c023f7b6f462 18 //SPI spi(PB_15, PB_14, PB_13);
benkatz 4:c023f7b6f462 19 //GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
benkatz 4:c023f7b6f462 20
benkatz 4:c023f7b6f462 21 //DigitalOut chipselect(PB_12);
benkatz 4:c023f7b6f462 22
benkatz 0:4e1c4df6aabd 23 using namespace FastMath;
benkatz 0:4e1c4df6aabd 24 using namespace Transforms;
benkatz 0:4e1c4df6aabd 25
benkatz 0:4e1c4df6aabd 26
benkatz 1:b8bceb4daed5 27 // Current Sampling IRQ
benkatz 2:8724412ad628 28 /*
benkatz 0:4e1c4df6aabd 29 extern "C" void TIM2_IRQHandler(void) {
benkatz 0:4e1c4df6aabd 30 // flash on update event
benkatz 0:4e1c4df6aabd 31 if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
benkatz 0:4e1c4df6aabd 32 inverter.SampleCurrent();
benkatz 0:4e1c4df6aabd 33 }
benkatz 0:4e1c4df6aabd 34 TIM2->SR = 0x0; // reset the status register
benkatz 0:4e1c4df6aabd 35 }
benkatz 2:8724412ad628 36 */
benkatz 2:8724412ad628 37 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 38 // toggle on update event
benkatz 2:8724412ad628 39 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 40 inverter.SampleCurrent();
benkatz 2:8724412ad628 41 }
benkatz 2:8724412ad628 42 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 43 //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
benkatz 2:8724412ad628 44 }
benkatz 0:4e1c4df6aabd 45
benkatz 0:4e1c4df6aabd 46
benkatz 0:4e1c4df6aabd 47 void Loop(void){
benkatz 0:4e1c4df6aabd 48 foc.Commutate();
benkatz 0:4e1c4df6aabd 49 }
benkatz 3:6a0015d88d06 50
benkatz 3:6a0015d88d06 51 void PrintStuff(void){
benkatz 3:6a0015d88d06 52 float velocity = encoder.GetMechVelocity();
benkatz 3:6a0015d88d06 53 float position = encoder.GetMechPosition();
benkatz 3:6a0015d88d06 54 printf("%f, %f;\n\r", position, velocity);
benkatz 3:6a0015d88d06 55 }
benkatz 0:4e1c4df6aabd 56 /*
benkatz 0:4e1c4df6aabd 57 void voltage_foc(void){
benkatz 0:4e1c4df6aabd 58 theta = encoder.GetElecPosition() + offset;
benkatz 0:4e1c4df6aabd 59 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 0:4e1c4df6aabd 60 spwm.Update_DTC(v_alpha, v_beta);
benkatz 0:4e1c4df6aabd 61 //output.write(theta/6.28318530718f);
benkatz 0:4e1c4df6aabd 62 }
benkatz 0:4e1c4df6aabd 63 */
benkatz 1:b8bceb4daed5 64
benkatz 0:4e1c4df6aabd 65
benkatz 0:4e1c4df6aabd 66 int main() {
benkatz 0:4e1c4df6aabd 67 wait(1);
benkatz 0:4e1c4df6aabd 68 testing.attach(&Loop, .0001);
benkatz 3:6a0015d88d06 69 //testing.attach(&PrintStuff, .05);
benkatz 3:6a0015d88d06 70 //inverter.SetDTC(.1, 0, 0);
benkatz 0:4e1c4df6aabd 71 //inverter.EnableInverter();
benkatz 0:4e1c4df6aabd 72 while(1) {
benkatz 3:6a0015d88d06 73 //printf("%f\n\r", encoder.GetElecPosition());
benkatz 3:6a0015d88d06 74 //wait(.1);
benkatz 0:4e1c4df6aabd 75 }
benkatz 0:4e1c4df6aabd 76 }