Diff: main.cpp

Revision:

14:80ce59119d93

Parent:

12:c473a25f54f7

Child:

15:ef00814e38e2

--- a/main.cpp	Sun May 22 03:47:40 2016 +0000
+++ b/main.cpp	Mon Oct 31 16:48:16 2016 +0000
@@ -19,12 +19,11 @@
 Serial pc(PA_2, PA_3);
 
 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....
+PositionSensorAM5147 spi(16384, 2.7f, 7);   ///1  I really need an eeprom or something to store this....
 //PositionSensorSPI spi(2048, 1.34f, 7); ///2
-int motorID = 40; ///1
-//int motorID = 50;  ///2
+
 
-PositionSensorEncoder encoder(1024, 0, 7); 
+PositionSensorEncoder encoder(4096, 0, 7); 
 
 
 
@@ -46,120 +45,40 @@
   TIM1->SR = 0x0; // reset the status register
 }
 
-// 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<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<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<200; i++){
-        //torqueController.SetTorque(.4);
-        inverter.SetDTC(dtc, 0, 0);
-        wait(0.00037921593);
-        //torqueController.SetTorque(-.4);
-        inverter.SetDTC(0, dtc, 0);
-        wait(0.00037921593);
-        }   
-        inverter.SetDTC(0, 0, 0);
-    wait(1);
-    for (int j = 0; j<3; j++){
-        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);
-
-        }
+int count = 0;
+void Loop(void){
+    count++;
+    //impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    impedanceController.SetImpedance(.1, -0.01, 0);
     
-    }
-
-
-/*      //sinusoidal voltage-mode control, for debugging.
-void voltage_foc(void){
-    float theta = encoder.GetElecPosition();
-    InvPark(v_d, v_q, theta, &v_alpha, &v_beta);
-    InvClarke(v_alpha, v_beta, &v_a, &v_b, &v_c);
-    svpwm.Update_DTC(v_a, v_b, v_c);
-    //output.write(theta/6.28318530718f);
-    }
-*/
-
-// For decoding serial commands.
- void serialInterrupt(void){
-     //wait(.001);
-     int i = 0;
-     while(pc.readable()){
-         buff[i] = pc.getc();
-         wait(.0001);
-         i++;
-         
-         }
-     int val = (buff[4]<<8) + buff[5];
-     int checksum = buff[2]^buff[3]^buff[4]^buff[5];
-     int validStart = (buff[0] == 255 && buff[1] == 255 && buff[2]==motorID && checksum==buff[6]);
-
-     if(validStart){
-
-                switch(buff[3]){
-                    case 10:
-                        cmd_float[1] = (float)val*val_max[1]/65278.0f;
-                        break;
-                    case 20:
-                        cmd_float[2] = (float)val*val_max[2]/65278.0f;
-                        break;
-                    case 30:
-                        cmd_float[0] = (float)val*val_max[0]/65278.0f;
-                        break;
-                        }
-                        }
-                        
-    
-    //pc.printf("%d  %d  %d  %d  %d  %d  %d \n", start1, start2, id, cmd, byte1, byte2, byte3);
-    //pc.printf("%f, %f, %f\n", cmd_float[0], cmd_float[1], cmd_float[2]);
-    //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, 0);
-    //torqueController.SetTorque(0);
+    //torqueController.SetTorque(-.03);
     //foc.Commutate();
     //voltage_foc();
+    if(count>2000){
+        //float e = spi.GetElecPosition();
+        //float v = encoder.GetMechVelocity();
+        //printf("%f\n\r", v);
+        //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
+        count = 0;
+        }
 
     }
 
 void PrintStuff(void){
+    //inverter.SetDTC(0.03, 0.0, 0.0);
+
     //float v = encoder.GetMechVelocity();
     //float position = encoder.GetElecPosition();
-    //float position = encoder.GetMechPosition();
+    int position = spi.GetRawPosition();
     //float m = spi.GetMechPosition();
-    //float e = spi.GetElecPosition();
-    //printf("%f\n\r", e);
+    float e = spi.GetElecPosition();
+    foc.Commutate();
+    float q = foc.GetQ();
+    printf("position: %d   angle: %f    q current: %f\n\r", position, e, q);
+    //inverter.getCurrent()
     //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]);
+    //printf("IA: %f   IB: %f  IC: %f\n\r", inverter.I_A, inverter.I_B, inverter.I_C);
     }
 
  /*
@@ -178,13 +97,14 @@
     inverter.DisableInverter();
     spi.ZeroPosition();
     wait(.1);
-    inverter.SetDTC(0.2, 0.2, 0.2);
+    inverter.SetDTC(0.03, 0.0, 0.0);
     inverter.EnableInverter();
-    //hk_start();
     foc.Reset();
-    testing.attach(&Loop, .0001);
+    testing.attach(&Loop, .000025);
+    //testing.attach(&PrintStuff, .05);
     NVIC_SetPriority(TIM5_IRQn, 2);
-    pc.baud(115200);
+    pc.baud(921600);
+    pc.printf("HobbyKing Cheeta v1.1\n\r");
     wait(.1);
     while(1) {