1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 56:4f73fa55baa0
- Parent:
- 54:d889fe62bc0c
diff -r 14ac00a5f43d -r 4f73fa55baa0 main.cpp --- 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); } }