Motor control for robots. More compact, less object-oriented revision.

Dependencies:   FastPWM3 mbed-dev-f303

Fork of Hobbyking_Cheetah_V1 by Ben Katz

Committer:
benkatz
Date:
Fri Feb 05 00:52:53 2016 +0000
Revision:
0:4e1c4df6aabd
Child:
1:b8bceb4daed5
It works!

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 0:4e1c4df6aabd 10 Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
benkatz 0:4e1c4df6aabd 11
benkatz 0:4e1c4df6aabd 12 CurrentRegulator foc(&inverter, &encoder, .005, .5);
benkatz 0:4e1c4df6aabd 13
benkatz 0:4e1c4df6aabd 14 Ticker testing;
benkatz 0:4e1c4df6aabd 15 //SPWM spwm(&inverter, 2.0);
benkatz 0:4e1c4df6aabd 16
benkatz 0:4e1c4df6aabd 17 //DigitalOut Dbg_pin(PA_15);
benkatz 0:4e1c4df6aabd 18
benkatz 0:4e1c4df6aabd 19 using namespace FastMath;
benkatz 0:4e1c4df6aabd 20 using namespace Transforms;
benkatz 0:4e1c4df6aabd 21
benkatz 0:4e1c4df6aabd 22 float offset = 0;//-0.24;
benkatz 0:4e1c4df6aabd 23 /*
benkatz 0:4e1c4df6aabd 24 float v_alpha = 0;
benkatz 0:4e1c4df6aabd 25 float v_beta = 0;
benkatz 0:4e1c4df6aabd 26
benkatz 0:4e1c4df6aabd 27 float i_d = 0;
benkatz 0:4e1c4df6aabd 28 float i_q = 1;
benkatz 0:4e1c4df6aabd 29 float v_d = 0;
benkatz 0:4e1c4df6aabd 30 float v_q = -.2;
benkatz 0:4e1c4df6aabd 31 float f;
benkatz 0:4e1c4df6aabd 32 float theta;
benkatz 0:4e1c4df6aabd 33 */
benkatz 0:4e1c4df6aabd 34 extern "C" void TIM2_IRQHandler(void) {
benkatz 0:4e1c4df6aabd 35 // flash on update event
benkatz 0:4e1c4df6aabd 36 if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
benkatz 0:4e1c4df6aabd 37 inverter.SampleCurrent();
benkatz 0:4e1c4df6aabd 38 }
benkatz 0:4e1c4df6aabd 39 TIM2->SR = 0x0; // reset the status register
benkatz 0:4e1c4df6aabd 40 }
benkatz 0:4e1c4df6aabd 41
benkatz 0:4e1c4df6aabd 42 /*
benkatz 0:4e1c4df6aabd 43 void PrintEncoder(void){
benkatz 0:4e1c4df6aabd 44 printf("%f\n\r", encoder.GetElecPosition());
benkatz 0:4e1c4df6aabd 45 //printf("%f\n\r", encoder.GetMechPosition());
benkatz 0:4e1c4df6aabd 46
benkatz 0:4e1c4df6aabd 47 //printf("%d\n\r", TIM3->CNT-0x8000);
benkatz 0:4e1c4df6aabd 48 }
benkatz 0:4e1c4df6aabd 49 */
benkatz 0:4e1c4df6aabd 50
benkatz 0:4e1c4df6aabd 51 void Loop(void){
benkatz 0:4e1c4df6aabd 52 foc.Commutate();
benkatz 0:4e1c4df6aabd 53 }
benkatz 0:4e1c4df6aabd 54 /*
benkatz 0:4e1c4df6aabd 55 void voltage_foc(void){
benkatz 0:4e1c4df6aabd 56 theta = encoder.GetElecPosition() + offset;
benkatz 0:4e1c4df6aabd 57 InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
benkatz 0:4e1c4df6aabd 58 spwm.Update_DTC(v_alpha, v_beta);
benkatz 0:4e1c4df6aabd 59 //output.write(theta/6.28318530718f);
benkatz 0:4e1c4df6aabd 60 }
benkatz 0:4e1c4df6aabd 61 */
benkatz 0:4e1c4df6aabd 62 /*
benkatz 0:4e1c4df6aabd 63 void open_loop(void){
benkatz 0:4e1c4df6aabd 64 f = t.read();
benkatz 0:4e1c4df6aabd 65 v_alpha = 0.1f*sin(10.0f*f);
benkatz 0:4e1c4df6aabd 66 v_beta = 0.1f*cos(10.0f*f);
benkatz 0:4e1c4df6aabd 67 spwm.Update_DTC(v_alpha, v_beta);
benkatz 0:4e1c4df6aabd 68 }
benkatz 0:4e1c4df6aabd 69 */
benkatz 0:4e1c4df6aabd 70
benkatz 0:4e1c4df6aabd 71 int main() {
benkatz 0:4e1c4df6aabd 72 wait(1);
benkatz 0:4e1c4df6aabd 73
benkatz 0:4e1c4df6aabd 74 testing.attach(&Loop, .0001);
benkatz 0:4e1c4df6aabd 75
benkatz 0:4e1c4df6aabd 76 //inverter.EnableInverter();
benkatz 0:4e1c4df6aabd 77
benkatz 0:4e1c4df6aabd 78 while(1) {
benkatz 0:4e1c4df6aabd 79
benkatz 0:4e1c4df6aabd 80 }
benkatz 0:4e1c4df6aabd 81 }