Yi-Ting Hsieh / Hobbyking_Cheetah_test0223

Test the set param mode

Dependencies:   FastPWM3

main.cpp@8:10ae7bc88d6e, 2016-04-13 (annotated)

Committer:
benkatz
Date:
Wed Apr 13 04:09:56 2016 +0000
Revision:
8:10ae7bc88d6e
Parent:
7:dc5f27756e02
Child:
9:d7eb815cb057
Multi-turn mechanical position, MA700 position sensing over SPI.  Placeholder torque controller for cogging/ripple compensation;

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