Shao Rui
1
Diff: main.cpp
- Revision:
- 6:3e6d09f56278
- Parent:
- 5:4aa2a7fa7818
- Child:
- 7:e3cff4376669
--- 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 ;
}