Gu Jasper
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 51:b0a3ef66ea3d
- Parent:
- 50:ba72df25d10f
- Child:
- 52:91a42bd0fe2e
--- a/main.cpp Thu Apr 04 13:53:58 2019 +0000
+++ b/main.cpp Wed May 08 01:17:38 2019 +0000
@@ -10,7 +10,7 @@
#define SETUP_MODE 4
#define ENCODER_MODE 5
-#define VERSION_NUM "1.8"
+#define VERSION_NUM "0.1"
float __float_reg[64]; // Floats stored in flash
@@ -52,12 +52,22 @@
//DigitalOut drv_en_gate(PA_11);
DRV832x drv(&drv_spi, &drv_cs);
-PositionSensorAM5147 spi(16384, 0.0, NPP);
+PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg
volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
+//================HJB=============added==========
+using namespace FastMath;
+volatile float Init_pos = 0;
+volatile float Pmag = 1;
+volatile float Tperiod = 25;
+volatile float p_des_HJB=0;
+volatile float v_des_HJB=0;
+
+//===================HJB end===================
+
void onMsgReceived() {
//msgAvailable = true;
printf("%df\n\r", rxMsg.id);
@@ -131,6 +141,7 @@
drv.enable_gd();
//gpio.enable->write(1);
controller.ovp_flag = 0;
+ controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off.
reset_foc(&controller); // Tesets integrators, and other control loop parameters
wait(.001);
controller.i_d_ref = 0;
@@ -155,13 +166,13 @@
}
void print_encoder(void){
- printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+ printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //spi.GetMechPosition
//printf("%d\n\r", spi.GetRawPosition());
wait(.001);
}
/// Current Sampling Interrupt ///
-/// This runs at 40 kHz, regardless of of the mode the controller is in ///
+/// This runs at 40 kHz, 25us, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
@@ -178,7 +189,7 @@
controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
controller.dtheta_elec = spi.GetElecVelocity();
- controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
+ controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //HJB find, new ADC? V_SCALE 0.012890625f
///
/// Check state machine state, and run the appropriate function ///
@@ -199,6 +210,9 @@
if(state_change){
enter_torque_mode();
count = 0;
+ //===============================================HJB added====================================================//
+ Init_pos = controller.theta_mech;
+ // printf(" Mechanical Angle: %f \n\r", Init_pos); //spi.GetMechPosition
}
else{
/*
@@ -211,15 +225,27 @@
printf("OVP Triggered!\n\r");
}
*/
-
+ //========================================HJB added for trajectory input=========================================//
+ Pmag = controller.PmagIn;
+ Tperiod = controller.TperiodIn;
+ p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count/(Tperiod*40000));//Pmag*FastSin(2*PI*count/(Tperiod*40000));
+ v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count/(Tperiod*40000))/Tperiod;
+ controller.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16);
+ controller.v_des = v_des_HJB;
+ if(count>=(Tperiod*40000))
+ {count = 0;}
+
+ //========================================HJB end=========================================//
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
controller.kp = 0;
controller.kd = 0;
controller.t_ff = 0;
+ //controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off.
+ controller.v_des = 0; //HJB added
}
-
+
torque_control(&controller);
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
@@ -251,7 +277,12 @@
void serial_interrupt(void){
while(pc.readable()){
char c = pc.getc();
- if(c == 27){
+ if(c == 27){
+ //===============================================HJB added====================================================//
+ wait_us(100); //HJB add, to clear fault
+ drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
+ Init_pos = controller.theta_mech; //Input the local mechanical theta
+ //===============================================HJB ended====================================================//
state = REST_MODE;
state_change = 1;
char_count = 0;
@@ -309,7 +340,7 @@
TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
break;
case 't':
- CAN_TIMEOUT = atoi(cmd_val);
+ CAN_TIMEOUT = atoi(cmd_val); //100000*25us= 2.5s
break;
default:
printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
@@ -355,24 +386,27 @@
}
int main() {
+ wait(1);
controller.v_bus = V_BUS;
+ controller.PmagIn = 1;
+ controller.TperiodIn = 10;
controller.mode = 0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
wait(.1);
- gpio.enable->write(1);
+ gpio.enable->write(1); //enable!
wait_us(100);
drv.calibrate();
wait_us(100);
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
wait_us(100);
- drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
+ drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); //SEN_LVL_1_0 0x3
wait_us(100);
drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
-
+
//drv.enable_gd();
zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
- drv.disable_gd();
+ drv.disable_gd(); //all mosfets in the Hi-z state
@@ -417,9 +451,9 @@
spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
init_controller_params(&controller);
- pc.baud(921600); // set serial baud rate
+ pc.baud(115200); // set serial baud rate
wait(.01);
- pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+ pc.printf("\n\r\n\r High-integated Joint tobe Better\n\r\n\r");
wait(.01);
printf("\n\r Debug Info:\n\r");
printf(" Firmware Version: %s\n\r", VERSION_NUM);
@@ -444,19 +478,43 @@
int counter = 0;
+ //===============================================HJB added====================================================//
+ wait_us(100); //HJB add, to clear fault
+ drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
+ //Init_pos = controller.theta_mech; //Input the local mechanical theta
+ //===============================================HJB ended====================================================//
while(1) {
drv.print_faults();
wait(.1);
//printf("%.4f\n\r", controller.v_bus);
- /*
+
if(state == MOTOR_MODE)
{
- //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
+ printf(" %.3f %.3f %.3f \n\r", controller.p_des,controller.p_des-controller.theta_mech,controller.v_des-controller.dtheta_mech); //spi.GetMechPosition
+ // 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);
- wait(.002);
+ // printf("%.3f\n\r", controller.dtheta_mech);
+ wait(.002);
+ }
+
+ /*
+ if(state == MOTOR_MODE)
+ {
+ //================HJB added=======================================================================================
+ printf("\n\Temperature Observer\n\r");
+ printf("%f %f \n\r\n\r", observer->temperature , observer->resistance);
+ //====HJB added end=================================================================================================
+ //========HJB added==================================================================================
+ float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
+ printf("\n\rCurrent\n\r");
+ printf("%f %f %f\n\r\n\r", controller->i_d_filt, controller->i_q_filt, current);
+ //====HJB added end======================================================================================
+ //================HJB added=======================================================================================
+ printf("\n\Id Iq refrence input Observer\n\r");
+ printf("%f %f %f\n\r\n\r", controller->i_d_ref , controller->i_q_ref, controller->v_bus);
+ //====HJB added end=================================================================================================
+ wait(.002);
}
*/
-
}
}