Diff: main.cpp

Revision:

9:d7eb815cb057

Parent:

8:10ae7bc88d6e

Child:

10:370851e6e132

--- a/main.cpp	Wed Apr 13 04:09:56 2016 +0000
+++ b/main.cpp	Tue May 10 01:15:57 2016 +0000
@@ -6,19 +6,40 @@
 #include "Transforms.h"
 #include "CurrentRegulator.h"
 #include "TorqueController.h"
-//#include "ImpedanceController.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);
+
+int id[3] = {0};
+float cmd_float[3] = {0.0f};
+int raw[3] = {0};
+float val_max[3] = {18.0f, 1.0f, 0.1f};
+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.01007080078, 0.00005);  //hall motter
+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);
+PositionSensorSPI spi(2048, 2.75f);   ///1  I really need an eeprom or something to store this....
+//PositionSensorSPI spi(2048, 1.34f); ///2
+//PositionSensorSPI spi(2048, 1);
+int motorID = 40; ///1
+//int motorID = 50;  ///2
+
+PositionSensorEncoder encoder(1024, 0);
 
 CurrentRegulator foc(&inverter, &spi, .005, .5);  //hall sensor
-TorqueController torqueController(.061f, &foc);
+TorqueController torqueController(.031f, &foc);
+ImpedanceController impedanceController(&torqueController, &spi, &encoder);
+
 //CurrentRegulator foc(&inverter, &encoder, .005, .5);    //test motter
+//SVPWM  svpwm(&inverter, 2.0f);
 
-//SVPWM  svpwm(&inverter, 2.0f);
 Ticker  testing;
 //Timer t;
 
@@ -31,6 +52,8 @@
 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
@@ -59,7 +82,7 @@
   if (TIM1->SR & TIM_SR_UIF ) {
       inverter.SampleCurrent();
       //wait(.00002);
-      //foc.Commutate();
+      //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
@@ -76,19 +99,102 @@
     //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]);
+        }
+        
+    }
+    
+*/
+
+ 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){
-    foc.Commutate();
+    
+    impedanceController.SetImpedance(cmd_float[1], cmd_float[2], cmd_float[0]);
+    //impedanceController.SetImpedance(-.4, -0.006, 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;
+        }
+    
+     
+    //torqueController.SetTorque(0);
+    //foc.Commutate();
     //voltage_foc();
     }
 
 void PrintStuff(void){
-    //float velocity = encoder.GetMechVelocity();
+    //float v = encoder.GetMechVelocity();
     //float position = encoder.GetElecPosition();
     //float position = encoder.GetMechPosition();
-    float m = spi.GetMechPosition();
+    //float m = spi.GetMechPosition();
     float e = spi.GetElecPosition();
-    //printf("%f, %f;\n\r", position, velocity);
-    printf("Elec:  %f \n\r", m);
+    printf("%f\n\r", e);
+    //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]);
     }
     
 
@@ -106,16 +212,31 @@
 */
        
 int main() {
-    //t.start();
-    wait(1);
+    //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();
+    foc.Reset();
     testing.attach(&Loop, .0001);
-    
-    NVIC_SetPriority(TIM5_IRQn, 1);
+    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.EnableInverter();
+    //inverter.DisableInverter();
     //foc.Commutate();
+    wait(.5);
     while(1) {
         //printf("%f\n\r", encoder.GetElecPosition());
         //wait(.1);