main.cpp

00001 #include "mbed.h"
00002 #include "PositionSensor.h"
00003 #include "Inverter.h"
00004 #include "SVM.h"
00005 #include "FastMath.h"
00006 #include "Transforms.h"
00007 #include "CurrentRegulator.h"
00008 #include "TorqueController.h"
00009 #include "ImpedanceController.h"
00010 
00011 using namespace FastMath;
00012 using namespace Transforms;
00013 
00014 int id[3] = {0};
00015 float cmd_float[3] = {0.0f};
00016 int raw[3] = {0};
00017 float val_max[3] = {18.0f, 1.0f, 0.1f};    //max angle in radians, stiffness in N-m/rad, damping in N-m*s/rad
00018 int buff[8];
00019 Serial pc(PA_2, PA_3);
00020 
00021 Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motor
00022 PositionSensorAM5147 spi(16384, 2.7f, 7);   ///1  I really need an eeprom or something to store this....
00023 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
00024 
00025 
00026 PositionSensorEncoder encoder(4096, 0, 7); 
00027 
00028 
00029 
00030 CurrentRegulator foc(&inverter, &spi, &encoder, 0.000033, .005, .5);    
00031 TorqueController torqueController(.031f, &foc);
00032 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
00033 
00034 Ticker  testing;
00035 
00036 
00037 
00038 
00039 // Current Sampling IRQ
00040 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00041   if (TIM1->SR & TIM_SR_UIF ) {
00042       inverter.SampleCurrent();
00043       //foc.Commutate(); ///Putting the loop here doesn't work for some reason.  Need to figure out why
00044       }
00045   TIM1->SR = 0x0; // reset the status register
00046 }
00047 
00048 int count = 0;
00049 void Loop(void){
00050     count++;
00051     //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
00052     impedanceController.SetImpedance(.1, -0.01, 0);
00053     
00054     //torqueController.SetTorque(-.03);
00055     //foc.Commutate();
00056     //voltage_foc();
00057     if(count>2000){
00058         //float e = spi.GetElecPosition();
00059         //float v = encoder.GetMechVelocity();
00060         //printf("%f\n\r", v);
00061         //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
00062         count = 0;
00063         }
00064 
00065     }
00066 
00067 void PrintStuff(void){
00068     //inverter.SetDTC(0.03, 0.0, 0.0);
00069 
00070     //float v = encoder.GetMechVelocity();
00071     //float position = encoder.GetElecPosition();
00072     int position = spi.GetRawPosition();
00073     //float m = spi.GetMechPosition();
00074     float e = spi.GetElecPosition();
00075     foc.Commutate();
00076     float q = foc.GetQ();
00077     printf("position: %d   angle: %f    q current: %f\n\r", position, e, q);
00078     //inverter.getCurrent()
00079     //printf("%f   %f   %f   %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
00080     //printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
00081     //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
00082     }
00083 
00084  /*
00085  ////Throw some sines on the phases.  useful to make sure the hardware works.
00086  void gen_sine(void){
00087      float f = 1.0f;
00088      float time = t.read();
00089      float a = .45f*sin(6.28318530718f*f*time) + .5f;
00090      float b = .45f*sin(6.28318530718f*f*time + 2.09439510239f) + .5f;
00091      float c = .45f*sin(6.28318530718f*f*time + 4.18879020479f) + .5f;
00092      inverter.SetDTC(a, b, c);
00093      }
00094 */
00095        
00096 int main() {
00097     inverter.DisableInverter();
00098     spi.ZeroPosition();
00099     wait(.1);
00100     inverter.SetDTC(0.03, 0.0, 0.0);
00101     inverter.EnableInverter();
00102     foc.Reset();
00103     testing.attach(&Loop, .000025);
00104     //testing.attach(&PrintStuff, .05);
00105     NVIC_SetPriority(TIM5_IRQn, 2);
00106     pc.baud(921600);
00107     pc.printf("HobbyKing Cheeta v1.1\n\r");
00108     wait(.1);
00109     while(1) {
00110 
00111     }
00112 }