Diff: main.cpp

Revision:

11:c83b18d41e54

Parent:

10:370851e6e132

Child:

12:c473a25f54f7

--- a/main.cpp	Thu May 12 05:02:52 2016 +0000
+++ b/main.cpp	Sun May 22 00:14:59 2016 +0000
@@ -8,120 +8,86 @@
 #include "TorqueController.h"
 #include "ImpedanceController.h"
 
-///SPI Input Stuff
-//DigitalIn cselect(PB_12);
-//InterruptIn select(PB_12);
-//DigitalIn mosi(PB_15);
-//SPISlave input(PB_15, PB_14, PB_13, PB_12);
+using namespace FastMath;
+using namespace Transforms;
 
 int id[3] = {0};
 float cmd_float[3] = {0.0f};
 int raw[3] = {0};
-float val_max[3] = {18.0f, 1.0f, 0.1f};
+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
 int buff[8];
 Serial pc(PA_2, PA_3);
 
-//PositionSensorEncoder encoder(8192,4.0f);
-//Inverter inverter(PA_5, PB_10, PB_3, PB_7, 0.02014160156, 0.00005);
-Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motter
-//Inverter inverter(PA_10, PA_9, PA_8, PB_7, 0.01007080078, 0.00005);  //test motter
-//PositionSensorSPI spi(2048, 2.75f, 7);   ///1  I really need an eeprom or something to store this....
+Inverter inverter(PA_10, PA_9, PA_8, PA_11, 0.02014160156, 0.00005);  //hall motor
+PositionSensorSPI spi(2048, 2.75f, 7);   ///1  I really need an eeprom or something to store this....
 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
-PositionSensorSPI spi(2048, 3.0, 21);
 int motorID = 40; ///1
 //int motorID = 50;  ///2
 
-//PositionSensorEncoder encoder(1024, 0, 7);
-PositionSensorEncoder encoder(1024, 0, 21);
+PositionSensorEncoder encoder(1024, 0, 7); 
 
 
-CurrentRegulator foc(&inverter, &spi, .005, .5);  //hall sensor
+
+CurrentRegulator foc(&inverter, &spi, .005, .5);    
 TorqueController torqueController(.031f, &foc);
 ImpedanceController impedanceController(&torqueController, &spi, &encoder);
 
-//CurrentRegulator foc(&inverter, &encoder, .005, .5);    //test motter
-//SVPWM  svpwm(&inverter, 2.0f);
-
 Ticker  testing;
-//Timer t;
 
-/*
-float v_d = 0;
-float v_q = .1;
-float v_alpha = 0;
-float v_beta = 0;
-float v_a = 0;
-float v_b = 0;
-float v_c = 0;
-*/
-float ref = 0.0;
-int count = 0;
 
-//SPI spi(PB_15, PB_14, PB_13);
-//GPIOB->MODER = (1 << 8); // set pin 4 to be general purpose output
-
-//DigitalOut chipselect(PB_12);
-
-using namespace FastMath;
-using namespace Transforms;
 
 
 // Current Sampling IRQ
-/*
-extern "C" void TIM2_IRQHandler(void) {
-  // flash on update event
-  if (TIM2->SR & TIM_SR_UIF & TIM2->CNT>0x465) {
-      inverter.SampleCurrent();
-      }
-  TIM2->SR = 0x0; // reset the status register
-}
-*/
-
-
-
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
-  // toggle on update event
   if (TIM1->SR & TIM_SR_UIF ) {
       inverter.SampleCurrent();
-      //wait(.00002);
       //foc.Commutate(); ///Putting the loop here doesn't work for some reason.  Need to figure out why
       }
   TIM1->SR = 0x0; // reset the status register
-    //GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
 }
 
-
+// HobbyKing-style startup tone.  Just because.
 void hk_start(void){
+    float dtc  = .1;
     inverter.SetDTC(0, 0, 0);
     inverter.EnableInverter();
-    for(int i = 0; i<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.000956);
-        torqueController.SetTorque(-.4);
-        wait(0.000956);
+    for(int i = 0; i<200; i++){
+        //torqueController.SetTorque(.4);
+        inverter.SetDTC(dtc, 0, 0);
+        wait(0.00047778308);
+        //torqueController.SetTorque(-.4);
+        inverter.SetDTC(0, dtc, 0);
+        wait(0.00047778308);
         }
-    for(int i = 0; i<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.0008513);
-        torqueController.SetTorque(-.4);
-        wait(0.0008513);
+    for(int i = 0; i<200; i++){
+        //torqueController.SetTorque(.4);
+        inverter.SetDTC(dtc, 0, 0);
+        wait(0.00042565508);
+        //torqueController.SetTorque(-.4);
+        inverter.SetDTC(0, dtc, 0);
+        wait(0.00042565508);
         }    
-    for(int i = 0; i<120; i++){
-        torqueController.SetTorque(.4);
-        wait(0.00075843);
-        torqueController.SetTorque(-.4);
-        wait(0.00075843);
+    for(int i = 0; i<200; i++){
+        //torqueController.SetTorque(.4);
+        inverter.SetDTC(dtc, 0, 0);
+        wait(0.00037921593);
+        //torqueController.SetTorque(-.4);
+        inverter.SetDTC(0, dtc, 0);
+        wait(0.00037921593);
         }   
-        torqueController.SetTorque(0);
-    wait(.4);
+        inverter.SetDTC(0, 0, 0);
+    wait(1);
     for (int j = 0; j<3; j++){
-        for(int i = 0; i<120; i++){
-            torqueController.SetTorque(.4);
-            wait(0.000956);
-            torqueController.SetTorque(-.4);
-            wait(0.000956);
+        for(int i = 0; i<240; i++){
+            //torqueController.SetTorque(.4);
+            inverter.SetDTC(dtc, 0, 0);
+            wait(0.00047778308);
+            //torqueController.SetTorque(-.4);
+            inverter.SetDTC(0, dtc, 0);
+            wait(0.00047778308);
             }   
             torqueController.SetTorque(0);
+            inverter.SetDTC(0, 0, 0);
             wait(.2);
 
         }
@@ -129,7 +95,7 @@
     }
 
 
-/*
+/*      //sinusoidal voltage-mode control, for debugging.
 void voltage_foc(void){
     float theta = encoder.GetElecPosition();
     InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
@@ -138,37 +104,8 @@
     //output.write(theta/6.28318530718f);
     }
 */
-/*
-void read(void){
-    int startByte;
-    if(input.receive()){
-        //startByte = input.read();
-        //if(startByte == 65535){
-            //startByte = input.read();
-            //wait(.000005);
-            raw[0] = input.read();
-            raw[1] = input.read();
-            raw[2] = input.read();
-            id[0] = raw[0]>>14;
-            id[1] = raw[1]>>14;
-            id[2] = raw[2]>>14;
-            printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
-            for(int i = 0; i<3; i++){
-                    cmd_float[id[i]] = (val_max[id[i]])*(float)(raw[i] - (id[i]<<14))/16383.0f;
-                    }
-           //  }
-         // else{
-          //    input.read();
-          //    input.read();
-           //   input.read();
-           //   }
-        //printf("%d   %d   %d \n\r", raw[0], raw[1], raw[2]);
-        }
-        
-    }
-    
-*/
 
+// For decoding serial commands.
  void serialInterrupt(void){
      //wait(.001);
      int i = 0;
@@ -203,26 +140,15 @@
     //pc.printf("%d\n", cmd); 
     //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d\n", buff[0], buff[1], buff[2], buff[3], buff[4], buff[5], buff[6], buff[7]);
      }
-         
+
 void Loop(void){
     
-    //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
-    impedanceController.SetImpedance(-.04, -0.00, 0);
-
-    count = count+1;
-    
-    if(count > 1000){
-        //ref= -1*ref;
-        //printf("%f   %f   %f \n\r",  cmd_float[0], cmd_float[1], cmd_float[2]);
-        //float e = spi.GetElecPosition();
-        //printf("%f\n\r", e);
-        count = 0;
-        }
-    
-     
+    impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    //impedanceController.SetImpedance(-.04, 0, 0);
     //torqueController.SetTorque(0);
     //foc.Commutate();
     //voltage_foc();
+
     }
 
 void PrintStuff(void){
@@ -235,11 +161,9 @@
     //printf("%f   %f   %f   %f \n\r", m, cmd_float[0], cmd_float[1], cmd_float[2]);
     //printf("%d   %d   %d\n\r", raw[0], raw[1], raw[2]);
     }
-    
 
-
- 
  /*
+ ////Throw some sines on the phases.  useful to make sure the hardware works.
  void gen_sine(void){
      float f = 1.0f;
      float time = t.read();
@@ -251,34 +175,18 @@
 */
        
 int main() {
-    //mosi.mode(PullDown);
-    //cselect.mode(PullUp);
     inverter.DisableInverter();
     spi.ZeroPosition();
-    //input.format(16, 0);
-    //input.frequency(100000);
-    //select.fall(&read);
-
-
-    //NVIC_SetPriority(EXTI15_10_IRQn, 1);
     wait(.1);
     inverter.SetDTC(0.2, 0.2, 0.2);
     inverter.EnableInverter();
-    hk_start();
-    //foc.Reset();
+    //hk_start();
+    foc.Reset();
     testing.attach(&Loop, .0001);
     NVIC_SetPriority(TIM5_IRQn, 2);
     pc.baud(115200);
-    //pc.attach(&serialInterrupt);
-    //printf("hello\n\r");
-    //testing.attach(&gen_sine, .01);
-    //testing.attach(&PrintStuff, .1);
-    //inverter.SetDTC(.05, 0, 0);
-    //inverter.DisableInverter();
-    //foc.Commutate();
-    wait(.5);
+    wait(.1);
     while(1) {
-        //printf("%f\n\r", encoder.GetElecPosition());
-        //wait(.1);
+
     }
 }