Shao Rui
1
main.cpp
- Committer:
- shaorui
- Date:
- 2020-07-20
- Revision:
- 7:e3cff4376669
- Parent:
- 6:3e6d09f56278
File content as of revision 7:e3cff4376669:
#include "mbed.h"
#include "math_ops.h"
#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
// length of outgoing/incoming messages
#define DATA_LEN 30
#define CMD_LEN 66
// Master CAN ID ///
#define CAN_ID 0x0
/// Value Limits ///
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -45.0f
#define V_MAX 45.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
//#define T_MIN -18.0f
//#define T_MAX 18.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 ///
#define A_LIM_P 5.0f//1.5f
#define A_LIM_N -5.0f//-1.5f
#define H_LIM_P 5.0f
#define H_LIM_N -5.0f
#define K_LIM_P 0.2f
#define K_LIM_N 7.7f
#define KP_SOFTSTOP 0.0f//100.0f
#define KD_SOFTSTOP 0.4f;
#define ENABLE_CMD 0xFFFF
#define DISABLE_CMD 0x1F1F
spi_data_t spi_data; // data from spine to up
spi_command_t spi_command; // data from up to spine
spi_command_t *mspi_command=&spi_command; // data from up to spine
// spi buffers
uint16_t rx_buff[RX_LEN];
uint16_t tx_buff[TX_LEN];
DigitalOut led(PC_5);
Serial pc(PA_2, PA_3);
CAN can1(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name
CAN can2(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
CANMessage rxMsg1, rxMsg2;
CANMessage txMsg1, txMsg2;
CANMessage a1_can, a2_can, h1_can, h2_can, k1_can, k2_can; //TX Messages
int ledState;
Ticker sendCAN;
volatile int counter = 0;
volatile bool msgAvailable = false;
Ticker loop;
int spi_enabled = 0;
InterruptIn cs(PA_4);
//**//DigitalIn estop(PA_14);
DigitalIn estop(PB_15);
//SPISlave spi(PA_7, PA_6, PA_5, PA_4);
leg_state l1_state, l2_state;;
leg_control l1_control, l2_control;
uint16_t x = 0;
uint16_t x2 = 0;
uint16_t count = 0;
uint16_t counter2 = 0;
int control_mode = 1;
int is_standing = 0;
int enabled = 0;
// generates fake spi data from spi command
void test_control();
void control();
//==========hjb=======//=================================================================================//
///////////////////////////////////////////////////////
#define MULTI200 1
#define MAX_TRACK 10240*5
#define MIN_SAMPLE 0.001
float TimeSample, TimeSample2;
#define TS (MULTI200 * MIN_SAMPLE) // Sample Time
#define SP 0.2 // Interval
float Fz_mea;//2.7
float trajRef[5][9], trajRef2[5][9]; // ref. Traj. 0..2 -> q_d, 3..5-> v_d, 6..8->a_d.
REF ref[5];
///////////////////////////////////////////////////////
//++++++++ FOURARM +++++++++++++++++++++++//
float g_ArmSpeed = 250;//2.50f; //控制关节速度
float g_ArmJointAngleRe[5];
float g_ArmJointAngleDe[5];
float g_ArmJointAngleLineDe[5];
float g_ArmJointAngleDe_Past[5] = {0,0,0,0,0};
float g_ArmJointAngleDe_PastP[5] = {0,0,0,0,0};
float g_ArmTrajRef[15];
float g_ArmTrajLineRef[15];
int g_ArmInit=TRUE;
static int Control_Flag = 0;
static int timeout = 0;
//++++++++ FOURARM +++++++++++++//
///////////////////////////////////////////////////////
//++++++++ FOURARM +++++++++++++++++++++++//
#define ARMSP 0.200 // Interval
#define ARMJOINTSPEED 2.5f //关节速度定义 2.5f
#define ARMJOINTACC 0.100f //关节加速度时间
//++++++++ FOURARM +++++++++++++++++++++++//
ARMREF g_ArmRef;
//++++++++ FOURARM +++++++++++++//
//================HJB=============added==========
using namespace FastMath;
volatile float Init_pos = 0;
volatile float Init_pos1 = 0;//shaorui add
volatile float Pmag = 1;
volatile float Pmag1 = 1;//shaorui add
volatile float Pmag_last = 1;
volatile float Pmag_last1 = 1;//shaorui add
volatile float Tperiod = 25;
volatile float Tperiod1= 25;//shaorui add
volatile float p_des_HJB=0;
volatile float p_des_shaorui=0;//shaorui add
volatile float v_des_HJB=0;
volatile float v_des_shaorui=0;//shaorui add
volatile int count_shaorui= 0;
volatile int count_HJB= 0; //shaorui add
//===================HJB end===================
//==========hjb=======//=================================================================================//
/// CAN Command Packet Structure ///
/// 16 bit position command, between -4*pi and 4*pi
/// 12 bit velocity command, between -30 and + 30 rad/s
/// 12 bit kp, between 0 and 500 N-m/rad
/// 12 bit kd, between 0 and 100 N-m*s/rad
/// 12 bit feed forward torque, between -18 and 18 N-m
/// CAN Packet is 8 8-bit words
/// Formatted as follows. For each quantity, bit 0 is LSB
/// 0: [position[15-8]]
/// 1: [position[7-0]]
/// 2: [velocity[11-4]]
/// 3: [velocity[3-0], kp[11-8]]
/// 4: [kp[7-0]]
/// 5: [kd[11-4]]
/// 6: [kd[3-0], torque[11-8]]
/// 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);
float kd = fminf(fmaxf(KD_MIN, uint_to_float(joint.kd, KD_MIN, KD_MAX, 12)), KD_MAX);
float t_ff = fminf(fmaxf(T_MIN, uint_to_float(joint.t_ff, T_MIN, T_MAX, 12)), T_MAX);
/// convert floats to unsigned ints ///
uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
/// pack ints into the can buffer ///
msg->data[0] = p_int>>8;
msg->data[1] = p_int&0xFF;
msg->data[2] = v_int>>4;
msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
msg->data[4] = kp_int&0xFF;
msg->data[5] = kd_int>>4;
msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
msg->data[7] = t_int&0xff;
///printf("p:%d v:%d kp:%d kd:%d t:%d ",(uint16_t)p_int, (uint16_t)v_int, kp_int, kd_int, t_int);
}
/// CAN Reply Packet Structure ///
/// 16 bit position, between -4*pi and 4*pi
/// 12 bit velocity, between -30 and + 30 rad/s
/// 12 bit current, between -40 and 40;
/// CAN Packet is 5 8-bit words
/// Formatted as follows. For each quantity, bit 0 is LSB
/// 0: [position[15-8]]
/// 1: [position[7-0]]
/// 2: [velocity[11-4]]
/// 3: [velocity[3-0], current[11-8]]
/// 4: [current[7-0]]
void unpack_reply(CANMessage msg, leg_state * leg){
/// unpack ints from can buffer ///
uint16_t id = msg.data[0];
uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
/// convert uints to floats ///
float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
if(id==1){
leg->a.p = p;
leg->a.v = v;
leg->a.t = t;
//printf("p=%.3f v=%.3f i=%.3f", p, v, t);
}
else if(id==2){
leg->h.p = p;
leg->h.v = v;
leg->h.t = t;
}
else if(id==3){
leg->k.p = p;
leg->k.v = v;
leg->k.t = t;
}
}
void rxISR1() {
can1.read(rxMsg1); // read message into Rx message storage
printf("%c\n",rxMsg1.id); //shaorui add
unpack_reply(rxMsg1, &l1_state);
}
void rxISR2(){
can2.read(rxMsg2);
unpack_reply(rxMsg2, &l2_state);
}
void PackAll(){
pack_cmd(&a1_can, l1_control.a);
pack_cmd(&a2_can, l2_control.a);
pack_cmd(&h1_can, l1_control.h);
pack_cmd(&h2_can, l2_control.h);
pack_cmd(&k1_can, l1_control.k);
pack_cmd(&k2_can, l2_control.k);
}
void WriteAll(){
//toggle = 1;
can1.write(a1_can);
wait(.0001);
can2.write(a2_can);
wait(.0002);
can1.write(h1_can);
wait(.0001);
can2.write(h2_can);
wait(.0002);
can1.write(k1_can);
wait(.0001);
can2.write(k2_can);
//wait(.0001);
//toggle = 0;
}
void sendCMD(){
Control_Flag = 1;
//=========================hjb ==============================================================//
//#define PI 3.1415926f
//static int init = TRUE;
//static double Tacc[3]={0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S
//static float MAX_STEP[3]={12.0, 12.0, 12.0}; // max step limitation 12 degree
//control();
WriteAll();
}
void Zero(CANMessage * msg){
msg->data[0] = 0xFF;
msg->data[1] = 0xFF;
msg->data[2] = 0xFF;
msg->data[3] = 0xFF;
msg->data[4] = 0xFF;
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFE;
WriteAll();
}
void EnterMotorMode(CANMessage * msg){
msg->data[0] = 0xFF;
msg->data[1] = 0xFF;
msg->data[2] = 0xFF;
msg->data[3] = 0xFF;
msg->data[4] = 0xFF;
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFC;
//WriteAll();
}
void ExitMotorMode(CANMessage * msg){
msg->data[0] = 0xFF;
msg->data[1] = 0xFF;
msg->data[2] = 0xFF;
msg->data[3] = 0xFF;
msg->data[4] = 0xFF;
msg->data[5] = 0xFF;
msg->data[6] = 0xFF;
msg->data[7] = 0xFD;
//WriteAll();
}
void serial_isr(){
/// handle keyboard commands from the serial terminal ///
while(pc.readable()){
char c = pc.getc();
led = !led;
switch(c){
case(27):
//loop.detach();
printf("\n\r exiting motor mode \n\r");
ExitMotorMode(&a1_can);
ExitMotorMode(&a2_can);
ExitMotorMode(&h1_can);
ExitMotorMode(&h2_can);
ExitMotorMode(&k1_can);
ExitMotorMode(&k2_can);
enabled = 0;
break;
case('m'):
printf("\n\r entering motor mode \n\r");
EnterMotorMode(&a1_can);
EnterMotorMode(&a2_can);
EnterMotorMode(&h1_can);
EnterMotorMode(&h2_can);
EnterMotorMode(&k1_can);
EnterMotorMode(&k2_can);
//WriteAll(); //hjb added
//wait(.5); //hjb delete
enabled = 1;
break;
case('s'):
printf("\n\r standing \n\r");
counter2 = 0;
is_standing = 1;
//stand();
break;
case('z'):
printf("\n\r zeroing \n\r");
Zero(&a1_can);
Zero(&a2_can);
Zero(&h1_can);
Zero(&h2_can);
Zero(&k1_can);
Zero(&k2_can);
break;
}
}
WriteAll();
}
uint16_t xor_checksum(uint16_t* data, size_t len)
{
uint16_t t = 0;
for(int i = 0; i < len; i++)
t = t ^ data[i];
return t;
}
void spi_isr(void)
{
//led = !led; // HJB added
timeout = 0;
GPIOC->ODR |= (1 << 8);
GPIOC->ODR &= ~(1 << 8);
int bytecount = 0;
// tx_buff[0]=0x1111; //hjb added
SPI1->DR = tx_buff[0];
while(cs == 0) {
if(SPI1->SR&0x1) {
rx_buff[bytecount] = SPI1->DR;
bytecount++;
if(bytecount<TX_LEN) {
SPI1->DR = tx_buff[bytecount];
}
}
}
// after reading, save into spi_command
// should probably check checksum first!
//uint16_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32); //===hjb del ====
uint16_t calc_checksum = xor_checksum(rx_buff,32);
// for(int i = 0; i < CMD_LEN; i++)
// {
// ((uint16_t*)(&spi_command))[i] = rx_buff[i];
//}
spi_command.q_des_abad[0] = rx_buff[0];
spi_command.q_des_hip[0] = rx_buff[2];
spi_command.q_des_knee[0] = rx_buff[4];
spi_command.qd_des_abad[0] = rx_buff[6];
spi_command.qd_des_hip[0] = rx_buff[8];
spi_command.qd_des_knee[0] = rx_buff[10];
spi_command.kp_abad[0] = rx_buff[12];
spi_command.kp_hip[0] = rx_buff[14];
spi_command.kp_knee[0] = rx_buff[16];
spi_command.kd_abad[0] = rx_buff[18];
spi_command.kd_hip[0] = rx_buff[20];
spi_command.kd_knee[0] = rx_buff[22];
spi_command.tau_abad_ff[0] = rx_buff[24];
spi_command.tau_hip_ff[0] = rx_buff[26];
spi_command.tau_knee_ff[0] = rx_buff[28];
spi_command.flags[0] = rx_buff[30];
spi_command.q_des_abad[1] = rx_buff[1];
spi_command.q_des_hip[1] = rx_buff[3];
spi_command.q_des_knee[1] = rx_buff[5];
spi_command.qd_des_abad[1] = rx_buff[7];
spi_command.qd_des_hip[1] = rx_buff[9];
spi_command.qd_des_knee[1] = rx_buff[11];
spi_command.kp_abad[1] = rx_buff[13];
spi_command.kp_hip[1] = rx_buff[15];
spi_command.kp_knee[1] = rx_buff[17];
spi_command.kd_abad[1] = rx_buff[19];
spi_command.kd_hip[1] = rx_buff[21];
spi_command.kd_knee[1] = rx_buff[23];
spi_command.tau_abad_ff[1] = rx_buff[25];
spi_command.tau_hip_ff[1] = rx_buff[27];
spi_command.tau_knee_ff[1] = rx_buff[29];
spi_command.flags[1] = rx_buff[31];
spi_command.checksum = rx_buff[32];
// run control, which fills in tx_buff for the next iteration
if(calc_checksum != spi_command.checksum){
spi_data.flags[1] = 0xdead;}
//test_control();
//spi_data.q_abad[0] = 13.0f;
//control();
//PackAll(); //hjb cancel
//WriteAll(); //hjb cancel
//sendCMD(); //hjb added
//for (int i=0; i<TX_LEN; i++) {
// printf("%d ", rx_buff[i]);
// printf("R= %.3f",*(&spi_command)[i]);
// }
//printf("\n\r");
}
int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ //**// exceed limit edge, using the impedance control
if((state.p)>=limit_p){
//control->p_des = limit_p;
control->v_des = 0.0f;
control->kp = 0;
control->kd = KD_SOFTSTOP;
control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
//control->t_ff = KP_SOFTSTOP*(limit_p - state.p); //hjb changed
return 1;
}
else if((state.p)<=limit_n){
//control->p_des = limit_n;
control->v_des = 0.0f;
control->kp = 0;
control->kd = KD_SOFTSTOP;
control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
//control->t_ff = KP_SOFTSTOP*(limit_n - state.p); //hjb changed
return 1;
}
return 0;
}
void control()
{
spi_data.q_abad[0] = l1_state.a.p;
spi_data.q_hip[0] = l1_state.h.p;
spi_data.q_knee[0] = l1_state.k.p;
spi_data.qd_abad[0] = l1_state.a.v;
spi_data.qd_hip[0] = l1_state.h.v;
spi_data.qd_knee[0] = l1_state.k.v;
spi_data.q_abad[1] = l2_state.a.p;
spi_data.q_hip[1] = l2_state.h.p;
spi_data.q_knee[1] = l2_state.k.p;
spi_data.qd_abad[1] = l2_state.a.v;
spi_data.qd_hip[1] = l2_state.h.v;
spi_data.qd_knee[1] = l2_state.k.v;
tx_buff[0] = float_to_uint(spi_data.q_abad[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[0]);
tx_buff[1] = float_to_uint(spi_data.q_abad[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[1]);
tx_buff[2] = float_to_uint(spi_data.q_hip[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[0]);
tx_buff[3] = float_to_uint(spi_data.q_hip[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[1]);
tx_buff[4] = float_to_uint(spi_data.q_knee[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[0]);
tx_buff[5] = float_to_uint(spi_data.q_knee[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[1]);
tx_buff[6] = float_to_uint(spi_data.qd_abad[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[0]);
tx_buff[7] = float_to_uint(spi_data.qd_abad[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[1]);
tx_buff[8] = float_to_uint(spi_data.qd_hip[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[0]);
tx_buff[9] = float_to_uint(spi_data.qd_hip[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[1]);
tx_buff[10] = float_to_uint(spi_data.qd_knee[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[0]);
tx_buff[11] = float_to_uint(spi_data.qd_knee[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[1]);
tx_buff[12] = (uint16_t)(spi_data.flags[0]);
tx_buff[13] = (uint16_t)(spi_data.flags[1]);
spi_data.checksum = xor_checksum(tx_buff,14);
tx_buff[14] = (spi_data.checksum)&0xFFFF;
//=================================================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] = {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((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
count_shaorui = 0; //for trajectory
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
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;
}
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];
//========================================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=========================================//
//========================================shaorui added for trajectory input(leg2)=========================================//
Pmag1 = uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16);
Tperiod1 = uint_to_float(spi_command.qd_des_abad[1], V_MIN, V_MAX, 12);//;
if (Pmag1 != Pmag_last1){count_shaorui = 0;}
Pmag_last1 = Pmag1;
Init_pos1 = 0;
p_des_shaorui = Init_pos1 + Pmag1*FastSin(2*PI*count_shaorui/(Tperiod1*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000));
v_des_shaorui = 2*PI*Pmag1*FastCos(2*PI*count_shaorui/(Tperiod1*1000))/Tperiod1;
l1_control.a.p_des = p_des_shaorui;//uint_to_float(p_des_HJB, -15.f, 15.f, 16);
l1_control.a.v_des = v_des_shaorui;
if(count_shaorui>=(Tperiod*1000)) {
count_shaorui= 0;
}
count_shaorui++;
//========================================shaorui 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();
}
}
//spi_data.checksum = xor_checksum((uint16_t*)&spi_data,14);
// for(int i = 0; i < DATA_LEN; i++){
// tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
}
void test_control()
{
for(int i = 0; i < 2; i++)
{
spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
spi_data.q_hip[i] = spi_command.q_des_hip[i] + 1.f;
spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
spi_data.qd_hip[i] = spi_command.qd_des_hip[i] + 1.f;
//printf("%d %d %f %.3f %.3f %.3f\n\r",(uint16_t)(spi_command.q_des_abad[i]),(uint16_t)spi_command.q_des_knee[i],spi_command.q_des_hip[i],spi_data.qd_abad[i],spi_data.qd_knee[i],spi_data.qd_hip[i]);
}
//spi_data.flags[0] = 0xdead;
//spi_data.flags[1] = 0xbeef;
spi_data.flags[0] = 2;
spi_data.flags[1] = 2;
// only do first 56 bytes of message.
//spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
tx_buff[0] = (uint16_t)(spi_data.q_abad[0]);
tx_buff[1] = (uint16_t)(spi_data.q_abad[1]);
tx_buff[2] = (uint16_t)(spi_data.q_hip[0]);
tx_buff[3] = (uint16_t)(spi_data.q_hip[1]);
tx_buff[4] = (uint16_t)(spi_data.q_knee[0]);
tx_buff[5] = (uint16_t)(spi_data.q_knee[1]);
tx_buff[6] = (uint16_t)(spi_data.qd_abad[0]);
tx_buff[7] = (uint16_t)(spi_data.qd_abad[1]);
tx_buff[8] = (uint16_t)(spi_data.qd_hip[0]);
tx_buff[9] = (uint16_t)(spi_data.qd_hip[1]);
tx_buff[10] = (uint16_t)(spi_data.qd_knee[0]);
tx_buff[11] = (uint16_t)(spi_data.qd_knee[1]);
tx_buff[12] = (uint16_t)(spi_data.flags[0]);
tx_buff[13] = (uint16_t)(spi_data.flags[1]);
spi_data.checksum = xor_checksum(tx_buff,14);
tx_buff[14] = (spi_data.checksum)&0xFFFF;
for(int i = 0; i < DATA_LEN; i++)
{ // tx_buff[i] = ((uint16_t*)(&spi_data))[i];
printf("%d ", tx_buff[i]);
}
int testchecksum = xor_checksum((uint16_t*)&spi_data,14);
printf("%d %d\n\r",testchecksum,spi_data.checksum);
}
void init_spi(void){
SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
spi->format(16, 0);
spi->frequency(12000000);
spi->reply(0x0);
cs.fall(&spi_isr);
printf("done\n\r");
}
int main() {
//wait(.5);
//led = 1;
//**// pc.baud(921600);
pc.baud(460800); //115200
pc.attach(&serial_isr);
estop.mode(PullUp);
//spi.format(16, 0);
//spi.frequency(1000000);
//spi.reply(0x0);
//cs.fall(&spi_isr);
can1.frequency(1000000); // set bit rate to 1Mbps
can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
can2.frequency(1000000); // set bit rate to 1Mbps
can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
memset(&spi_data, 0, sizeof(spi_data_t));
memset(&spi_command,0,sizeof(spi_command_t));
NVIC_SetPriority(TIM5_IRQn, 1);
NVIC_SetPriority(CAN1_RX0_IRQn, 4);
NVIC_SetPriority(CAN2_RX0_IRQn, 3);
/*
//=============================hjb===============================================================//
//ISR Setup
#define CAN_ARR 0x56 /// timer autoreload value 0x8CA
NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up
TIM1->CR1 |= TIM_CR1_UDIS;
TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
TIM1->RCR |= 0x001; // update event once per up/down count of tim1
TIM1->EGR |= TIM_EGR_UG;
//PWM Setup
TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
TIM1->ARR = CAN_ARR; // set auto reload, 40 khz
TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1
//++++++++ FOURARM ++++++++++++++++++++++//
//----------------------------------------------------------
int i = 0;
for (i=0; i<5; i++)
{
for (k=0; k<3; k++) trajRef[i][k] = 0;
for (k=0; k<3; k++) trajRef2[i][k] = 0;
}
//----------------------------------------------------------
//----------------------------------------------------------
for (i=0; i<5; i++)
{
for (k=0; k<3; k++)
{
ref[i].T1[k] = 0; // ?????????? time t //
ref[i].t1[k] = 0; // ?????????? time t //
ref[i].t[k] = 0; // current time t //
ref[i].M[k] = 0; // Desire Position C //
ref[i].N[k] = 0; // Desire Position C //
ref[i].L[k] = 0; // Last Position B //
ref[i].det_M[k] = 0; // C - B //
ref[i].det_L[k] = 0; // A - B //
}
}
//----------------------------------------------------------
*/
int i, k;
//++++++++++++++++++ FOURARM ++++++++++++++++++++//
for (i=0; i<5; i++)
{
g_ArmTrajRef[i] = 0.0f;
g_ArmJointAngleRe[i] = 0.0f;
g_ArmJointAngleDe[i] = 0.0f;
}
for (k=0; k<5; k++)
{
g_ArmRef.T1[k] = 0.0f; // ?????????? time t //
g_ArmRef.t1[k] = 0.0f; // ?????????? time t //
g_ArmRef.t[k] = 0.0f; // current time t //
g_ArmRef.M[k] = 0.0f; // Desire Position C //
g_ArmRef.N[k] = 0.0f; // Desire Position C //
g_ArmRef.L[k] = 0.0f; // Last Position B //
g_ArmRef.det_M[k] = 0.0f; // C - B //
g_ArmRef.det_L[k] = 0.0f; // A - B //
}
//++++++++++++++++++ FOURARM ++++++++++++++++++++//
//=============================hjb===============================================================//
printf("\n\r SPIne\n\r");
//printf("%d\n\r", RX_ID << 18);
a1_can.len = 8; //transmit 8 bytes
a2_can.len = 8; //transmit 8 bytes
h1_can.len = 8;
h2_can.len = 8;
k1_can.len = 8;
k2_can.len = 8;
rxMsg1.len = 6; //receive 6 bytes
rxMsg2.len = 6; //receive 6 bytes
a1_can.id = 0x1;
a2_can.id = 0x1;
h1_can.id = 0x2;
h2_can.id = 0x2;
k1_can.id = 0x3;
k2_can.id = 0x3;
pack_cmd(&a1_can, l1_control.a);
pack_cmd(&a2_can, l2_control.a);
pack_cmd(&h1_can, l1_control.h);
pack_cmd(&h2_can, l2_control.h);
pack_cmd(&k1_can, l1_control.k);
pack_cmd(&k2_can, l2_control.k);
WriteAll();
// SPI doesn't work if enabled while the CS pin is pulled low
// Wait for CS to not be low, then enable SPI
if(!spi_enabled){
while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
init_spi();
spi_enabled = 1;
}
loop.attach(&sendCMD, .001); //========hjb added========//
/*if(spi_enabled){
loop.attach(&sendCMD, .001); //============hjb added===========//
}
*/
while(1) {
//counter++;
//can2.read(rxMsg2);
//unpack_reply(rxMsg2, &l2_state);
//can1.read(rxMsg1); // read message into Rx message storage
//unpack_reply(rxMsg1, &l1_state);
if(Control_Flag){
control();
counter ++;
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);//shaorui delete
//printf("%.3f %.3f\n\r", l1_control.h.p_des, g_ArmJointAngleDe[1]);
counter = 0 ;
}
Control_Flag =0;
}
//wait_us(10);
}
}