Revision 56:4f73fa55baa0, committed 2020-05-14

Comitter:

shaorui

Date:

Thu May 14 10:44:41 2020 +0000

Parent:

55:14ac00a5f43d

Commit message:

1

Changed in this revision

--- a/Config/hw_config.h	Tue Nov 19 08:54:35 2019 +0000
+++ b/Config/hw_config.h	Thu May 14 10:44:41 2020 +0000
@@ -1,11 +1,11 @@
 #ifndef HW_CONFIG_H
 #define HW_CONFIG_H
-
-#define PIN_U PA_10
+//Be used in hw_setup.cpp
+#define PIN_U PA_8
 #define PIN_V PA_9
-#define PIN_W PA_8
-#define ENABLE_PIN PA_11        // Enable gate drive pin
-#define LED         PC_5        // LED Pin
+#define PIN_W PA_10
+#define ENABLE_PIN PC_9        // Enable gate drive pin
+#define LED         PC_7        // LED Pin
 #define I_SCALE 0.02014160156f  // Amps per A/D Count
 #define V_SCALE 0.012890625f     // Bus volts per A/D Count
 #define DTC_MAX 0.94f          // Max phase duty cycle

--- a/PositionSensor/PositionSensor.cpp	Tue Nov 19 08:54:35 2019 +0000
+++ b/PositionSensor/PositionSensor.cpp	Thu May 14 10:44:41 2020 +0000
@@ -11,11 +11,11 @@
     _ppairs = ppairs;
     ElecOffset = offset;
     rotations = 0;
-    spi = new SPI(PC_12, PC_11, PC_10);
+    spi = new SPI(PB_5, PB_4, PB_3);  //MOSI    MISO    SCK (Electrical Board spi1)
     spi->format(16, 1);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
-    spi->frequency(25000000);
+    spi->frequency(25000000);  
     
-    cs = new DigitalOut(PA_15);
+    cs = new DigitalOut(PD_2);  //spi1 CS
     cs->write(1);
     readAngleCmd = 0xffff;   
     MechOffset = offset;
@@ -26,12 +26,12 @@
     }
     
 void PositionSensorAM5147::Sample(float dt){
-    GPIOA->ODR &= ~(1 << 15);
+    GPIOD->ODR &= ~(1 << 2);
     raw = spi->write(readAngleCmd);
     raw &= 0x3FFF;   
     //raw = spi->write(0);
     //raw = raw>>2;                                                             //Extract last 14 bits
-    GPIOA->ODR |= (1 << 15);
+    GPIOD->ODR |= (1 << 2);
     int off_1 = offset_lut[raw>>7];
     int off_2 = offset_lut[((raw>>7)+1)%128];
     int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Position_Torque_Sensor/Position_Torque_Sensor.cpp	Thu May 14 10:44:41 2020 +0000
@@ -0,0 +1,300 @@
+
+#include "mbed.h"
+#include "Position_Torque_Sensor.h"
+#include "../math_ops.h"
+//#include "offset_lut.h"
+//#include <math.h>
+
+SensorAM5147::SensorAM5147(int CPR, float offset, int ppairs){
+    //_CPR = CPR;
+    _CPR = CPR;
+    _ppairs = ppairs;
+    ElecOffset = offset;
+    rotations = 0;
+    spi = new SPI(PB_15, PB_14, PB_13);  //MOSI    MISO    SCK (Electrical Board spi2)
+    spi->format(16, 1);                                                          // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
+    spi->frequency(25000000); 
+    //spi->frequency(10000000); 
+    
+    cs2 = new DigitalOut(PB_12);  //spi2 CS2
+    cs1 = new DigitalOut(PB_10);  //spi2 CS1
+    cs1->write(1);
+    readAngleCmd = 0xffff;   
+    MechOffset = offset;
+    modPosition = 0;
+    oldModPosition = 0;
+    oldVel = 0;
+    raw = 0;
+    }
+    
+void SensorAM5147::Sample(float dt){
+    GPIOB->ODR &= ~(1 << 10);
+    
+    raw = spi->write(readAngleCmd);
+    raw &= 0x3FFF;   
+    //raw = spi->write(0);
+    //raw = raw>>1;                                                             //Extract last 14 bits
+    GPIOB->ODR |= (1 << 10);
+    int off_1 = offset_lut[raw>>7];
+    int off_2 = offset_lut[((raw>>7)+1)%128];
+    int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
+    int angle = raw + off_interp;                                               // Correct for nonlinearity with lookup table from calibration
+    if(angle - old_counts > _CPR/2){
+        rotations -= 1;
+        }
+    else if (angle - old_counts < -_CPR/2){
+        rotations += 1;
+        }
+    
+    old_counts = angle;
+    oldModPosition = modPosition;
+    modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
+    position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
+    MechPosition = position - MechOffset;
+    float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
+    if(elec < 0) elec += 2.0f*PI;
+    else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 
+    ElecPosition = elec;
+    
+    float vel;
+    //if(modPosition<.1f && oldModPosition>6.1f){
+
+    if((modPosition-oldModPosition) < -3.0f){
+        vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
+        }
+    //else if(modPosition>6.1f && oldModPosition<0.1f){
+    else if((modPosition - oldModPosition) > 3.0f){
+        vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
+        }
+    else{
+        vel = (modPosition-oldModPosition)/dt;
+    }    
+    
+    int n = 40;
+    float sum = vel;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = vel;
+    MechVelocity =  sum/((float)n);
+    ElecVelocity = MechVelocity*_ppairs;
+    ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
+    }
+
+int SensorAM5147::GetRawPosition(){
+    return raw;
+    }
+
+float SensorAM5147::GetMechPositionFixed(){
+    return MechPosition+MechOffset;
+    }
+    
+float SensorAM5147::GetMechPosition(){
+    return MechPosition;
+    }
+
+float SensorAM5147::GetElecPosition(){
+    return ElecPosition;
+    }
+
+float SensorAM5147::GetElecVelocity(){
+    return ElecVelocity;
+    }
+
+float SensorAM5147::GetMechVelocity(){
+    return MechVelocity;
+    }
+
+void SensorAM5147::ZeroPosition(){
+    rotations = 0;
+    MechOffset = 0;
+    Sample(.00025f);
+    MechOffset = GetMechPosition();
+    }
+    
+void SensorAM5147::SetElecOffset(float offset){
+    ElecOffset = offset;
+    }
+void SensorAM5147::SetMechOffset(float offset){
+    MechOffset = offset;
+    }
+
+int SensorAM5147::GetCPR(){
+    return _CPR;
+    }
+
+
+void SensorAM5147::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }
+    
+
+
+SensorEncoder::SensorEncoder(int CPR, float offset, int ppairs) {
+    _ppairs = ppairs;
+    _CPR = CPR;
+    _offset = offset;
+    MechPosition = 0;
+    out_old = 0;
+    oldVel = 0;
+    raw = 0;
+    
+    // Enable clock for GPIOA
+    __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
+ 
+    GPIOA->MODER   |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ;           //PA6 & PA7 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
+    GPIOA->OTYPER  |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ;                 //PA6 & PA7 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
+    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ;     //Low speed                         /*!< GPIO port output speed register,       Address offset: 0x08      */
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ;           //Pull Down                         /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
+    GPIOA->AFR[0]  |= 0x22000000 ;                                          //AF02 for PA6 & PA7                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //nibbles here refer to gpio8..15   /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+   
+    // configure TIM3 as Encoder input
+    // Enable clock for TIM3
+    __TIM3_CLK_ENABLE();
+ 
+    TIM3->CR1   = 0x0001;                                                   // CEN(Counter ENable)='1'     < TIM control register 1
+    TIM3->SMCR  = TIM_ENCODERMODE_TI12;                                     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM3->CCMR1 = 0x1111;                                                   // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1, maximum digital filtering
+    TIM3->CCMR2 = 0x0000;                                                   //                             < TIM capture/compare mode register 2
+    TIM3->CCER  = 0x0011;                                                   // CC1P CC2P                   < TIM capture/compare enable register
+    TIM3->PSC   = 0x0000;                                                   // Prescaler = (0+1)           < TIM prescaler
+    TIM3->ARR   = CPR;                                                      // IM auto-reload register
+  
+    TIM3->CNT = 0x000;  //reset the counter before we use it  
+    
+    // Extra Timer for velocity measurement
+    
+    __TIM2_CLK_ENABLE();
+    TIM3->CR2 = 0x030;                                                      //MMS = 101
+    
+    TIM2->PSC = 0x03;
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->SMCR = 0x24;                                                      //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
+    TIM2->CCMR1 = 0x3;                                                      // CC1S = 11, IC1 mapped on TRC
+    
+    //TIM2->CR2 |= TIM_CR2_TI1S;
+    TIM2->CCER |= TIM_CCER_CC1P;
+    //TIM2->CCER |= TIM_CCER_CC1NP;
+    TIM2->CCER |= TIM_CCER_CC1E;
+    
+    
+    TIM2->CR1 = 0x01;                                                       //CEN,  enable timer
+    
+    TIM3->CR1   = 0x01;                                                     // CEN
+    ZPulse = new InterruptIn(PC_4);
+    ZSense = new DigitalIn(PC_4);
+    //ZPulse = new InterruptIn(PB_0);
+    //ZSense = new DigitalIn(PB_0);
+    ZPulse->enable_irq();
+    ZPulse->rise(this, &SensorEncoder::ZeroEncoderCount);
+    //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
+    ZPulse->mode(PullDown);
+    flag = 0;
+
+    
+    //ZTest = new DigitalOut(PC_2);
+    //ZTest->write(1);
+    }
+    
+void SensorEncoder::Sample(float dt){
+    
+    }
+
+ 
+float SensorEncoder::GetMechPosition() {                            //returns rotor angle in radians.
+    int raw = TIM3->CNT;
+    float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
+    return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
+}
+
+float SensorEncoder::GetElecPosition() {                            //returns rotor electrical angle in radians.
+    int raw = TIM3->CNT;
+    float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
+    if(elec < 0) elec += 6.28318530718f;
+    return elec;
+}
+
+
+    
+float SensorEncoder::GetMechVelocity(){
+
+    float out = 0;
+    float rawPeriod = TIM2->CCR1; //Clock Ticks
+    int currentTime = TIM2->CNT;
+    if(currentTime > 2000000){rawPeriod = currentTime;}
+    float  dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;    // +/- 1
+    float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 
+    if(isinf(meas)){ meas = 1;}
+    out = meas;
+    //if(meas == oldVel){
+     //   out = .9f*out_old;
+     //   }
+    
+ 
+    oldVel = meas;
+    out_old = out;
+    int n = 16;
+    float sum = out;
+    for (int i = 1; i < (n); i++){
+        velVec[n - i] = velVec[n-i-1];
+        sum += velVec[n-i];
+        }
+    velVec[0] = out;
+    return sum/(float)n;
+    }
+    
+float SensorEncoder::GetElecVelocity(){
+    return _ppairs*GetMechVelocity();
+    }
+    
+void SensorEncoder::ZeroEncoderCount(void){
+    if (ZSense->read() == 1 & flag == 0){
+        if (ZSense->read() == 1){
+            GPIOC->ODR ^= (1 << 4);   
+            TIM3->CNT = 0x000;
+            //state = !state;
+            //ZTest->write(state);
+            GPIOC->ODR ^= (1 << 4);
+            //flag = 1;
+        }
+        }
+    }
+
+void SensorEncoder::ZeroPosition(void){
+    
+    }
+    
+void SensorEncoder::ZeroEncoderCountDown(void){
+    if (ZSense->read() == 0){
+        if (ZSense->read() == 0){
+            GPIOC->ODR ^= (1 << 4);
+            flag = 0;
+            float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
+            if(dir != dir){
+                dir = dir;
+                rotations +=  dir;
+                }
+
+            GPIOC->ODR ^= (1 << 4);
+
+        }
+        }
+    }
+void SensorEncoder::SetElecOffset(float offset){
+    
+    }
+    
+int SensorEncoder::GetRawPosition(void){
+    return 0;
+    }
+    
+int SensorEncoder::GetCPR(){
+    return _CPR;
+    }
+    
+
+void SensorEncoder::WriteLUT(int new_lut[128]){
+    memcpy(offset_lut, new_lut, sizeof(offset_lut));
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Position_Torque_Sensor/Position_Torque_Sensor.h	Thu May 14 10:44:41 2020 +0000
@@ -0,0 +1,69 @@
+#ifndef POSITION_TORQUE_SENSOR_H
+#define POSITION_TORQUE_SENSOR_H
+class PositionSensor1 {
+public:
+    virtual void Sample(float dt) = 0;
+    virtual float GetMechPosition() {return 0.0f;}
+    virtual float GetMechPositionFixed() {return 0.0f;}
+    virtual float GetElecPosition() {return 0.0f;}
+    virtual float GetMechVelocity() {return 0.0f;}
+    virtual float GetElecVelocity() {return 0.0f;}
+    virtual void ZeroPosition(void) = 0;
+    virtual int GetRawPosition(void) = 0;
+    virtual void SetElecOffset(float offset) = 0;
+    virtual int GetCPR(void) = 0;
+    virtual  void WriteLUT(int new_lut[128]) = 0;
+};
+  
+  
+class SensorEncoder: public PositionSensor1 {
+public:
+    SensorEncoder(int CPR, float offset, int ppairs);
+    virtual void Sample(float dt);
+    virtual float GetMechPosition();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
+    virtual void ZeroPosition(void);
+    virtual void SetElecOffset(float offset);
+    virtual int GetRawPosition(void);
+    virtual int GetCPR(void);
+    virtual  void WriteLUT(int new_lut[128]); 
+private:
+    InterruptIn *ZPulse;
+    DigitalIn *ZSense;
+    //DigitalOut *ZTest;
+    virtual void ZeroEncoderCount(void);
+    virtual void ZeroEncoderCountDown(void);
+    int _CPR, flag, rotations, _ppairs, raw;
+    //int state;
+    float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40];
+    int offset_lut[128];
+};
+
+class SensorAM5147: public PositionSensor1{
+public:
+    SensorAM5147(int CPR, float offset, int ppairs);
+    virtual void Sample(float dt);
+    virtual float GetMechPosition();
+    virtual float GetMechPositionFixed();
+    virtual float GetElecPosition();
+    virtual float GetMechVelocity();
+    virtual float GetElecVelocity();
+    virtual int GetRawPosition();
+    virtual void ZeroPosition();
+    virtual void SetElecOffset(float offset);
+    virtual void SetMechOffset(float offset);
+    virtual int GetCPR(void);
+    virtual void WriteLUT(int new_lut[128]);
+private:
+    float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt;
+    int raw, _CPR, rotations, old_counts, _ppairs;
+    SPI *spi;
+    DigitalOut *cs1,*cs2;
+    int readAngleCmd;
+    int offset_lut[128];
+
+};
+
+#endif
\ No newline at end of file

--- a/hw_setup.cpp	Tue Nov 19 08:54:35 2019 +0000
+++ b/hw_setup.cpp	Thu May 14 10:44:41 2020 +0000
@@ -50,9 +50,9 @@
     
      ADC->CCR = 0x00000016;                                     // Regular simultaneous mode only
      ADC1->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC1 ON
-     ADC1->SQR3 = 0x000000A;                                    // use PC_0 as input- ADC1_IN0
+     ADC1->SQR3 = 0x000000D;                                    // use PC_3 as input- ADC1_IN0
      ADC2->CR2 |= ADC_CR2_ADON;//0x00000001;                    // ADC2 ON
-     ADC2->SQR3 = 0x0000000B;                                   // use PC_1 as input - ADC2_IN11
+     ADC2->SQR3 = 0x0000000C;                                   // use PC_2 as input - ADC2_IN11
      ADC3->CR2 |= ADC_CR2_ADON;                                 // ADC3 ON
      ADC3->SQR3 = 0x00000000;                                   // use PA_0, - ADC3_IN0
      GPIOC->MODER |= 0x0000000f;                                // Alternate function, PC_0, PC_1 are analog inputs 

--- 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);
     }
 }