1
Diff: main.cpp
- Revision:
- 6:3e6d09f56278
- Parent:
- 5:4aa2a7fa7818
- Child:
- 7:e3cff4376669
diff -r 4aa2a7fa7818 -r 3e6d09f56278 main.cpp --- a/main.cpp Fri Aug 02 08:25:09 2019 +0000 +++ b/main.cpp Tue Nov 19 09:00:12 2019 +0000 @@ -5,6 +5,10 @@ #include <cstring> #include "leg_message.h" #include "referenceTraj.h" +#include "FastMath.h" //hjb added +//#include "rtos.h" //hjb added + +//Thread thread; //hjb added // length of receive/transmit buffers #define RX_LEN 66 #define TX_LEN 66 @@ -29,8 +33,8 @@ //#define T_MIN -18.0f //#define T_MAX 18.0f - #define T_MIN -1.0f //hjb changed test - #define T_MAX 1.0f + #define T_MIN -3.0f //hjb changed test + #define T_MAX 3.0f #define SPI_TIMEOUT 1000 // hjb added for spi time out /// Joint Soft Stops /// @@ -66,7 +70,7 @@ CANMessage a1_can, a2_can, h1_can, h2_can, k1_can, k2_can; //TX Messages int ledState; Ticker sendCAN; -int counter = 0; +volatile int counter = 0; volatile bool msgAvailable = false; Ticker loop; @@ -129,6 +133,17 @@ //++++++++ FOURARM +++++++++++++++++++++++// ARMREF g_ArmRef; //++++++++ FOURARM +++++++++++++// +//================HJB=============added========== +using namespace FastMath; +volatile float Init_pos = 0; +volatile float Pmag = 1; +volatile float Pmag_last = 1; +volatile float Tperiod = 25; +volatile float p_des_HJB=0; +volatile float v_des_HJB=0; +volatile int count_HJB = 0; +//===================HJB end=================== + //==========hjb=======//=================================================================================// @@ -153,10 +168,12 @@ /// 7: [torque[7-0]] void pack_cmd(CANMessage * msg, joint_control joint){ - + /// limit data to be within bounds /// //float p_des = fminf(fmaxf(P_MIN, uint_to_float(joint.p_des, P_MIN, P_MAX, 16)), P_MAX); //float v_des = fminf(fmaxf(V_MIN, uint_to_float(joint.v_des, V_MIN, V_MAX, 12)), V_MAX); + //float p_des = fminf(fmaxf(P_MIN, p_des_HJB), P_MAX); + //float v_des = fminf(fmaxf(V_MIN, v_des_HJB), V_MAX); float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX); float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX); float kp = fminf(fmaxf(KP_MIN, uint_to_float(joint.kp, KP_MIN, KP_MAX, 12)), KP_MAX); @@ -245,15 +262,17 @@ can1.write(a1_can); wait(.0001); can2.write(a2_can); - wait(.0001); + wait(.0002); can1.write(h1_can); wait(.0001); can2.write(h2_can); - wait(.0001); + wait(.0002); + can1.write(k1_can); wait(.0001); can2.write(k2_can); - wait(.0001); + //wait(.0001); + //toggle = 0; } @@ -335,9 +354,10 @@ EnterMotorMode(&h2_can); EnterMotorMode(&k1_can); EnterMotorMode(&k2_can); - wait(.5); + //WriteAll(); //hjb added + //wait(.5); //hjb delete enabled = 1; - //loop.attach(&sendCMD, .001); //========hjb added========// + break; case('s'): printf("\n\r standing \n\r"); @@ -358,6 +378,7 @@ } WriteAll(); + } uint16_t xor_checksum(uint16_t* data, size_t len) @@ -370,7 +391,7 @@ void spi_isr(void) { - led = !led; // HJB added + //led = !led; // HJB added timeout = 0; GPIOC->ODR |= (1 << 8); GPIOC->ODR &= ~(1 << 8); @@ -522,62 +543,71 @@ //=================================================hjb added==========================// static double g_ArmTacc[5] = {0.060, 0.060, 0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S - static float g_ArmMAX_STEP[5] = {12.0, 12.0, 12.0, 12.0, 12.0}; + static float g_ArmMAX_STEP[5] = {20.0, 20.0, 20.0, 20.0, 20.0}; //static float Accr1 = 0.0,Accr2 = 0.0,Accr3 = 0.0,Accr4 = 0.0,Accr5 = 0.0; int k =0; for (k=0; k<5; k++) g_ArmTacc[k] = ARMJOINTACC; //0.030f; for (k=0; k<5; k++) g_ArmMAX_STEP[k] = g_ArmSpeed*2.0f*g_ArmTacc[k]; timeout++; - - if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){ - enabled = 1; - EnterMotorMode(&a1_can); - //can1.write(a1_can); - //wait(0.0001); - EnterMotorMode(&a2_can); - //can2.write(a2_can); - //wait(0.0001); - EnterMotorMode(&k1_can); - //can1.write(k1_can); - //wait(0.0001); - EnterMotorMode(&k2_can); - //can2.write(k2_can); - //wait(0.0001); - EnterMotorMode(&h1_can); - //can1.write(h1_can); - //wait(0.0001); - EnterMotorMode(&h2_can); - //can2.write(h2_can); - //wait(0.0001); - printf("e\n\r"); - return; - } - else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ //**//exit motor mode - enabled = 0; - ExitMotorMode(&a1_can); - //can1.write(a1_can); - ExitMotorMode(&a2_can); - //can2.write(a2_can); - ExitMotorMode(&h1_can); - //can1.write(h1_can); - ExitMotorMode(&h2_can); - //can2.write(h2_can); - ExitMotorMode(&k1_can); - //can1.write(k1_can); - ExitMotorMode(&k2_can); - //can2.write(k2_can); - printf("x\n\r"); - return; - } - - if(enabled){ - if(estop==0){ - //printf("estopped!!!!\n\r"); - //enabled = 0; //hjb added + if((timeout > SPI_TIMEOUT) && (SPI_TIMEOUT > 0)){ + enabled = 0; memset(&l1_control, 0, sizeof(l1_control)); memset(&l2_control, 0, sizeof(l2_control)); ExitMotorMode(&a1_can); //can1.write(a1_can); + //wait(0.0001); + ExitMotorMode(&a2_can); + //can2.write(a2_can); + //wait(0.0001); + ExitMotorMode(&k1_can); + //can1.write(k1_can); + //wait(0.0001); + ExitMotorMode(&k2_can); + //can2.write(k2_can); + //wait(0.0001); + ExitMotorMode(&h1_can); + //can1.write(h1_can); + //wait(0.0001); + ExitMotorMode(&h2_can); + //can2.write(k2_can); + //printf("Time out\n\r"); + spi_data.flags[0] = 0xdead; + spi_data.flags[1] = 0xdead; + led = 1; // HJB added + return; + } + else + { + if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){ + //===============================================HJB added====================================================// + Init_pos = spi_data.q_abad[0]; //==== initial the first position + count_HJB = 0; //for trajectory hjb + enabled = 1; + EnterMotorMode(&a1_can); + can1.write(a1_can); + wait(0.0001); + EnterMotorMode(&a2_can); + can2.write(a2_can); + wait(0.0001); + EnterMotorMode(&k1_can); + can1.write(k1_can); + wait(0.0001); + EnterMotorMode(&k2_can); + can2.write(k2_can); + wait(0.0001); + EnterMotorMode(&h1_can); + can1.write(h1_can); + wait(0.0001); + EnterMotorMode(&h2_can); + can2.write(h2_can); + wait(0.0001); + printf("e\n\r"); + return; + } + else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ //**//exit motor mode + enabled = 0; + ExitMotorMode(&a1_can); + //can1.write(a1_can); ExitMotorMode(&a2_can); //can2.write(a2_can); ExitMotorMode(&h1_can); @@ -588,117 +618,139 @@ //can1.write(k1_can); ExitMotorMode(&k2_can); //can2.write(k2_can); - - spi_data.flags[0] = 0xdead; - spi_data.flags[1] = 0xdead; - led = 1; // HJB added - + printf("x\n\r"); return; - } - else if((timeout > SPI_TIMEOUT) && (SPI_TIMEOUT > 0)){ - //enabled = 0; - EnterMotorMode(&a1_can); - //can1.write(a1_can); - //wait(0.0001); - EnterMotorMode(&a2_can); - //can2.write(a2_can); - //wait(0.0001); - EnterMotorMode(&k1_can); - //can1.write(k1_can); - //wait(0.0001); - EnterMotorMode(&k2_can); - //can2.write(k2_can); - //wait(0.0001); - EnterMotorMode(&h1_can); - //can1.write(h1_can); - //wait(0.0001); - EnterMotorMode(&h2_can); - //can2.write(k2_can); - //printf("Time out\n\r"); - return; - } - - else{ - led = 0; - memset(&l1_control, 0, sizeof(l1_control)); - memset(&l2_control, 0, sizeof(l2_control)); + } + } + if(enabled){ + if(estop==0){ + //printf("estopped!!!!\n\r"); + //enabled = 0; //hjb added + memset(&l1_control, 0, sizeof(l1_control)); + memset(&l2_control, 0, sizeof(l2_control)); + ExitMotorMode(&a1_can); + //can1.write(a1_can); + ExitMotorMode(&a2_can); + //can2.write(a2_can); + ExitMotorMode(&h1_can); + //can1.write(h1_can); + ExitMotorMode(&h2_can); + //can2.write(h2_can); + ExitMotorMode(&k1_can); + //can1.write(k1_can); + ExitMotorMode(&k2_can); + //can2.write(k2_can); + + spi_data.flags[0] = 0xdead; + spi_data.flags[1] = 0xdead; + led = 1; // HJB added + + return; + } + + - g_ArmJointAngleDe[0] =uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); //input angle from platform - g_ArmJointAngleDe[1] =uint_to_float(spi_command.q_des_hip[0], P_MIN, P_MAX, 16);//spi_command.q_des_hip[0]; - g_ArmJointAngleDe[2] =uint_to_float(spi_command.q_des_knee[0], P_MIN, P_MAX, 16);//spi_command.q_des_knee[0]; - g_ArmJointAngleDe[3] =0; - g_ArmJointAngleDe[4] =0; - - g_ArmJointAngleRe[0] = l1_state.a.p; - g_ArmJointAngleRe[1] = l1_state.h.p; - g_ArmJointAngleRe[2] = l1_state.k.p; - g_ArmJointAngleRe[3] = 0; - g_ArmJointAngleRe[4] = 0; - arm_reference_trajectory(g_ArmInit, TS, ARMSP, g_ArmTacc, &g_ArmRef, g_ArmMAX_STEP, g_ArmJointAngleRe, g_ArmJointAngleDe, g_ArmTrajRef); - - g_ArmInit = FALSE; + else{ + led = 0; + memset(&l1_control, 0, sizeof(l1_control)); + memset(&l2_control, 0, sizeof(l2_control)); + + g_ArmJointAngleDe[0] =uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); //input angle from platform + g_ArmJointAngleDe[1] =uint_to_float(spi_command.q_des_hip[0], P_MIN, P_MAX, 16);//spi_command.q_des_hip[0]; + g_ArmJointAngleDe[2] =uint_to_float(spi_command.q_des_knee[0], P_MIN, P_MAX, 16);//spi_command.q_des_knee[0]; + g_ArmJointAngleDe[3] =uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16); ; + g_ArmJointAngleDe[4] =uint_to_float(spi_command.q_des_hip[1], P_MIN, P_MAX, 16); + + g_ArmJointAngleRe[0] = l1_state.a.p; + g_ArmJointAngleRe[1] = l1_state.h.p; + g_ArmJointAngleRe[2] = l1_state.k.p; + g_ArmJointAngleRe[3] = l2_state.a.p; + g_ArmJointAngleRe[4] = l2_state.h.p; + arm_reference_trajectory(g_ArmInit, TS, ARMSP, g_ArmTacc, &g_ArmRef, g_ArmMAX_STEP, g_ArmJointAngleRe, g_ArmJointAngleDe, g_ArmTrajRef); + + g_ArmInit = FALSE; + + l1_control.a.p_des = g_ArmTrajRef[0]; + l1_control.h.p_des = g_ArmTrajRef[1]; + l1_control.k.p_des = g_ArmTrajRef[2]; + l2_control.a.p_des = g_ArmTrajRef[3]; + l2_control.h.p_des = g_ArmTrajRef[4]; + + l1_control.a.v_des = g_ArmTrajRef[5]; + l1_control.h.v_des = g_ArmTrajRef[6]; + l1_control.k.v_des = g_ArmTrajRef[7]; + l2_control.a.v_des = g_ArmTrajRef[8]; + l2_control.h.v_des = g_ArmTrajRef[9]; - l1_control.a.p_des = g_ArmTrajRef[0]; - l1_control.h.p_des = g_ArmTrajRef[1]; - l1_control.k.p_des = g_ArmTrajRef[2]; - - l1_control.a.v_des = g_ArmTrajRef[5]; - l1_control.h.v_des = g_ArmTrajRef[6]; - l1_control.k.v_des = g_ArmTrajRef[7]; + //========================================HJB added for trajectory input=========================================// + Pmag = uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); + Tperiod = uint_to_float(spi_command.qd_des_abad[0], V_MIN, V_MAX, 12);//; + if (Pmag != Pmag_last){count_HJB = 0;} + Pmag_last = Pmag; + Init_pos = 0; + p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count_HJB/(Tperiod*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000)); + v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count_HJB/(Tperiod*1000))/Tperiod; + l1_control.a.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16); + l1_control.a.v_des = v_des_HJB; + if(count_HJB>=(Tperiod*1000)) { + count_HJB = 0; + } + count_HJB++; + //========================================HJB end=========================================// + //l1_control.a.p_des = spi_command.q_des_abad[0]; + //l1_control.a.v_des = spi_command.qd_des_abad[0]; + l1_control.a.kp = spi_command.kp_abad[0]; + l1_control.a.kd = spi_command.kd_abad[0]; + l1_control.a.t_ff = spi_command.tau_abad_ff[0]; + + //l1_control.h.p_des = spi_command.q_des_hip[0]; + //l1_control.h.v_des = spi_command.qd_des_hip[0]; + l1_control.h.kp = spi_command.kp_hip[0]; + l1_control.h.kd = spi_command.kd_hip[0]; + l1_control.h.t_ff = spi_command.tau_hip_ff[0]; + + //l1_control.k.p_des = spi_command.q_des_knee[0]; + //l1_control.k.v_des = spi_command.qd_des_knee[0]; + l1_control.k.kp = spi_command.kp_knee[0]; + l1_control.k.kd = spi_command.kd_knee[0]; + l1_control.k.t_ff = spi_command.tau_knee_ff[0]; + + //l2_control.a.p_des = spi_command.q_des_abad[1]; + //l2_control.a.v_des = spi_command.qd_des_abad[1]; + l2_control.a.kp = spi_command.kp_abad[1]; + l2_control.a.kd = spi_command.kd_abad[1]; + l2_control.a.t_ff = spi_command.tau_abad_ff[1]; + + //l2_control.h.p_des = spi_command.q_des_hip[1]; + //l2_control.h.v_des = spi_command.qd_des_hip[1]; + l2_control.h.kp = spi_command.kp_hip[1]; + l2_control.h.kd = spi_command.kd_hip[1]; + l2_control.h.t_ff = spi_command.tau_hip_ff[1]; + + l2_control.k.p_des = spi_command.q_des_knee[1]; + l2_control.k.v_des = spi_command.qd_des_knee[1]; + l2_control.k.kp = spi_command.kp_knee[1]; + l2_control.k.kd = spi_command.kd_knee[1]; + l2_control.k.t_ff = spi_command.tau_knee_ff[1]; + + ///printf("ap=%d akp=%d at=%d \n\r",(uint16_t)l1_control.a.p_des,(uint16_t)l1_control.a.kp,(uint16_t)l1_control.a.t_ff); + + spi_data.flags[0] = 0; + spi_data.flags[1] = 0; + //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); //hjb cancelled + //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled + ////spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2; + //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); //hjb cancelled + //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled + ////spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2; + + //spi_data.flags[0] = 0xbeef; + //spi_data.flags[1] = 0xbeef; + PackAll(); + //WriteAll(); + } - - //l1_control.a.p_des = spi_command.q_des_abad[0]; - //l1_control.a.v_des = spi_command.qd_des_abad[0]; - l1_control.a.kp = spi_command.kp_abad[0]; - l1_control.a.kd = spi_command.kd_abad[0]; - l1_control.a.t_ff = spi_command.tau_abad_ff[0]; - - //l1_control.h.p_des = spi_command.q_des_hip[0]; - //l1_control.h.v_des = spi_command.qd_des_hip[0]; - l1_control.h.kp = spi_command.kp_hip[0]; - l1_control.h.kd = spi_command.kd_hip[0]; - l1_control.h.t_ff = spi_command.tau_hip_ff[0]; - - //l1_control.k.p_des = spi_command.q_des_knee[0]; - //l1_control.k.v_des = spi_command.qd_des_knee[0]; - l1_control.k.kp = spi_command.kp_knee[0]; - l1_control.k.kd = spi_command.kd_knee[0]; - l1_control.k.t_ff = spi_command.tau_knee_ff[0]; - - l2_control.a.p_des = spi_command.q_des_abad[1]; - l2_control.a.v_des = spi_command.qd_des_abad[1]; - l2_control.a.kp = spi_command.kp_abad[1]; - l2_control.a.kd = spi_command.kd_abad[1]; - l2_control.a.t_ff = spi_command.tau_abad_ff[1]; - - l2_control.h.p_des = spi_command.q_des_hip[1]; - l2_control.h.v_des = spi_command.qd_des_hip[1]; - l2_control.h.kp = spi_command.kp_hip[1]; - l2_control.h.kd = spi_command.kd_hip[1]; - l2_control.h.t_ff = spi_command.tau_hip_ff[1]; - - l2_control.k.p_des = spi_command.q_des_knee[1]; - l2_control.k.v_des = spi_command.qd_des_knee[1]; - l2_control.k.kp = spi_command.kp_knee[1]; - l2_control.k.kd = spi_command.kd_knee[1]; - l2_control.k.t_ff = spi_command.tau_knee_ff[1]; - - ///printf("ap=%d akp=%d at=%d \n\r",(uint16_t)l1_control.a.p_des,(uint16_t)l1_control.a.kp,(uint16_t)l1_control.a.t_ff); - - spi_data.flags[0] = 0; - spi_data.flags[1] = 0; - spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); //hjb cancelled - spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled - //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2; - spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); //hjb cancelled - spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled - //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2; - - //spi_data.flags[0] = 0xbeef; - //spi_data.flags[1] = 0xbeef; - PackAll(); - //WriteAll(); - } + } //spi_data.checksum = xor_checksum((uint16_t*)&spi_data,14); @@ -777,7 +829,7 @@ //wait(.5); //led = 1; //**// pc.baud(921600); - pc.baud(115200); + pc.baud(460800); //115200 pc.attach(&serial_isr); estop.mode(PullUp); //spi.format(16, 0); @@ -798,7 +850,7 @@ NVIC_SetPriority(TIM5_IRQn, 1); - NVIC_SetPriority(CAN1_RX0_IRQn, 3); + NVIC_SetPriority(CAN1_RX0_IRQn, 4); NVIC_SetPriority(CAN2_RX0_IRQn, 3); /* //=============================hjb===============================================================// @@ -910,10 +962,11 @@ init_spi(); spi_enabled = 1; } - if(spi_enabled){ + loop.attach(&sendCMD, .001); //========hjb added========// + /*if(spi_enabled){ loop.attach(&sendCMD, .001); //============hjb added===========// } - + */ while(1) { //counter++; //can2.read(rxMsg2); @@ -923,10 +976,14 @@ if(Control_Flag){ control(); - counter ++; + counter ++; + - if(counter>500){ - printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", l1_control.a.p_des, l1_state.a.p, l1_control.h.p_des, l1_state.h.p, l1_control.a.v_des, l1_state.a.v, l1_control.h.v_des, l1_state.h.v); + if(counter>100){ + + //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", l1_control.a.p_des*Deg_rad, l1_state.a.p*Deg_rad, l1_control.h.p_des*Deg_rad, l1_state.h.p*Deg_rad, l1_control.a.v_des*Deg_rad, l1_state.a.v*Deg_rad, l1_control.h.v_des*Deg_rad, l1_state.h.v*Deg_rad); + printf("%.3f %.3f %.3f %.3f %d\n\r", p_des_HJB*Deg_rad, l1_state.a.p*Deg_rad, v_des_HJB*Deg_rad, l1_state.a.v*Deg_rad, count_HJB); + //printf("%.3f %.3f\n\r", l1_control.h.p_des, g_ArmJointAngleDe[1]); counter = 0 ; }