Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

main.cpp@3:6a0015d88d06, 2016-03-09 (annotated)

Committer:
benkatz
Date:
Wed Mar 09 04:00:48 2016 +0000
Revision:
3:6a0015d88d06
Parent:
2:8724412ad628
Child:
4:c023f7b6f462
more motter;

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 0:4e1c4df6aabd 17 using namespace FastMath;
benkatz 0:4e1c4df6aabd 18 using namespace Transforms;
benkatz 0:4e1c4df6aabd 19
benkatz 1:b8bceb4daed5 20 //float offset = 0;//-0.24;
benkatz 0:4e1c4df6aabd 21
benkatz 1:b8bceb4daed5 22 // Current Sampling IRQ
benkatz 2:8724412ad628 23 /*
benkatz 0:4e1c4df6aabd 24 extern "C" void TIM2_IRQHandler(void) {
benkatz 0:4e1c4df6aabd 25 // flash on update event
benkatz 0:4e1c4df6aabd 26 if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
benkatz 0:4e1c4df6aabd 27 inverter.SampleCurrent();
benkatz 0:4e1c4df6aabd 28 }
benkatz 0:4e1c4df6aabd 29 TIM2->SR = 0x0; // reset the status register
benkatz 0:4e1c4df6aabd 30 }
benkatz 2:8724412ad628 31 */
benkatz 2:8724412ad628 32 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 33 // toggle on update event
benkatz 2:8724412ad628 34 if (TIM1->SR & TIM_SR_UIF ) {
benkatz 2:8724412ad628 35 inverter.SampleCurrent();
benkatz 2:8724412ad628 36 }
benkatz 2:8724412ad628 37 TIM1->SR = 0x0; // reset the status register
benkatz 2:8724412ad628 38 //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
benkatz 2:8724412ad628 39 }
benkatz 0:4e1c4df6aabd 40
benkatz 0:4e1c4df6aabd 41
benkatz 0:4e1c4df6aabd 42 void Loop(void){
benkatz 0:4e1c4df6aabd 43 foc.Commutate();
benkatz 0:4e1c4df6aabd 44 }
benkatz 3:6a0015d88d06 45
benkatz 3:6a0015d88d06 46 void PrintStuff(void){
benkatz 3:6a0015d88d06 47 float velocity = encoder.GetMechVelocity();
benkatz 3:6a0015d88d06 48 float position = encoder.GetMechPosition();
benkatz 3:6a0015d88d06 49 printf("%f, %f;\n\r", position, velocity);
benkatz 3:6a0015d88d06 50 }
benkatz 0:4e1c4df6aabd 51 /*
benkatz 0:4e1c4df6aabd 52 void voltage_foc(void){
benkatz 0:4e1c4df6aabd 53 theta = encoder.GetElecPosition() + offset;
benkatz 0:4e1c4df6aabd 54 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 0:4e1c4df6aabd 55 spwm.Update_DTC(v_alpha, v_beta);
benkatz 0:4e1c4df6aabd 56 //output.write(theta/6.28318530718f);
benkatz 0:4e1c4df6aabd 57 }
benkatz 0:4e1c4df6aabd 58 */
benkatz 1:b8bceb4daed5 59
benkatz 0:4e1c4df6aabd 60
benkatz 0:4e1c4df6aabd 61 int main() {
benkatz 0:4e1c4df6aabd 62 wait(1);
benkatz 0:4e1c4df6aabd 63
benkatz 0:4e1c4df6aabd 64 testing.attach(&Loop, .0001);
benkatz 3:6a0015d88d06 65 //testing.attach(&PrintStuff, .05);
benkatz 3:6a0015d88d06 66 //inverter.SetDTC(.1, 0, 0);
benkatz 0:4e1c4df6aabd 67 //inverter.EnableInverter();
benkatz 0:4e1c4df6aabd 68
benkatz 0:4e1c4df6aabd 69 while(1) {
benkatz 3:6a0015d88d06 70 //printf("%f\n\r", encoder.GetElecPosition());
benkatz 3:6a0015d88d06 71 //wait(.1);
benkatz 0:4e1c4df6aabd 72 }
benkatz 0:4e1c4df6aabd 73 }