Diff: main.cpp

Revision:

56:4f73fa55baa0

Parent:

54:d889fe62bc0c

--- a/main.cpp	Tue Nov 19 08:54:35 2019 +0000
+++ b/main.cpp	Thu May 14 10:44:41 2020 +0000
@@ -32,11 +32,13 @@
 #include "PreferenceWriter.h"
 #include "CAN_com.h"
 #include "DRV.h"
+#include "Position_Torque_Sensor.h"
  
 PreferenceWriter prefs(6);
 
 GPIOStruct gpio;
 ControllerStruct controller;
+ControllerStruct waijie_controller;
 ObserverStruct observer;
 COMStruct com;
 Serial pc(PA_2, PA_3);
@@ -47,18 +49,19 @@
 CANMessage   txMsg;
 
 
-SPI drv_spi(PA_7, PA_6, PA_5);
-DigitalOut drv_cs(PA_4);
-//DigitalOut drv_en_gate(PA_11);
+SPI drv_spi(PC_12, PC_11, PC_10); //MOSI  MISO  CLK  (Electrical Board spi3)
+DigitalOut drv_cs(PA_15);
+//DigitalOut drv_en_gate(PC_9);
 DRV832x drv(&drv_spi, &drv_cs);
 
 PositionSensorAM5147 spi(16384, 0.0, NPP);  
+SensorAM5147 spi1(16384,0.0,NPP);
 
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
 volatile int counthjb = 0;
-
+volatile int testcount = 0;
 void onMsgReceived() {
     //msgAvailable = true;
     counthjb++;
@@ -83,6 +86,12 @@
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
+            //Test for Can Communication
+            printf("p_des :%.3f\n\r",controller.p_des);
+            printf("v_des :%.3f\n\r",controller.v_des);
+            printf("kp :%.3f\n\r",controller.kp);
+            printf("kd :%.3f\n\r",controller.kd);
+            printf("t_ff :%.3f\n\r",controller.t_ff);
             }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
@@ -177,7 +186,8 @@
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
 
-        spi.Sample(DT);                                                           // sample position sensor
+        spi.Sample(DT);                                                         // sample position sensor
+        spi1.Sample(DT);                                                        // sample position sensor
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
@@ -187,7 +197,26 @@
         controller.dtheta_elec = spi.GetElecVelocity();
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
         ///
+        /************Test for AM5147P Position Sensor*********/
+                                                              
+        //controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
+        //controller.adc1_raw = ADC1->DR;
+        //controller.adc3_raw = ADC3->DR;
+        waijie_controller.theta_elec = spi1.GetElecPosition();
+        waijie_controller.theta_mech = spi1.GetMechPosition();
+        waijie_controller.dtheta_mech =spi1.GetMechVelocity();  
+        waijie_controller.dtheta_elec = spi1.GetElecVelocity();
+        //controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
         
+        testcount++;
+        if(testcount==40000)
+        {
+            printf("waijie:%.3f Raw:%.3f\n\r", waijie_controller.theta_mech,spi1.GetRawPosition());
+            printf("neizhi:%.3f  Raw:%.3f\n\r", GR*controller.theta_mech,spi.GetRawPosition());
+            testcount=0;
+        }
+        
+        /************Test for AM5147P Position Sensor*********/
         /// Check state machine state, and run the appropriate function ///
         switch(state){
             case REST_MODE:                                                     // Do nothing
@@ -431,7 +460,8 @@
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     init_controller_params(&controller);
 
-    pc.baud(460800);                                                            // set serial baud rate
+    //pc.baud(460800); 
+    pc.baud(115200);                                                           // set serial baud rate
     wait(.01);
     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
     wait(.01);
@@ -473,9 +503,14 @@
             //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
             //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
             //printf("%.3f\n\r", controller.dtheta_mech);
+            /********Test for AM5147P Position Sensor********/
+           // printf("%.3f\n\r", GR*waijie_controller.theta_mech);
+            /********Test for AM5147P Position Sensor********/
+            printf("enter enter enter\n\r");
             wait(.002);
         }
         
-
+        //pc.printf("%.3f\n\r", GR*waijie_controller.theta_mech);
+        //wait(.1);
     }
 }