/** *go控制程序 */ #include "GO_M8010_6_Driver.h" #include "usart.h" GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据 const float gravit_const =9.81;//计算前馈力矩有关 //数据处理函数 static void GO_M8010_recv_data(uint8_t* Temp_buffer); // void GO_M8010_recv_data(uint8_t* Temp_buffer); /** *@brief 电机初始化 */ void GO_M8010_init (){ for (uint8_t id= 0; id >8) & 0xFF))) { HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮 // indicate CRC incorrect HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭 return; } // CRC pass and start converting data to the motor HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); //红色灯灭 GO_M8010_recv_data(Temp_buffer); } /** *@brief 用户自定义串口发送完成中断回调函数 */ void uartTxCB(UART_HandleTypeDef *huart) { //发送完成后开启接受dma,并将传输完成中断回调函数指向串口接收完成中断回调函数 HAL_UART_Receive_DMA(huart,Temp_buffer,sizeof(Temp_buffer)); } /** *@brief go发送控制命令 *@input[in] huart 所用串口指针 *@input[in] id 被控电机id *@input[in] rev 暂时不知道干啥用的,github为回答issue *@input[in] T 力矩,单位N·m *@input[in] Pos 目标位置,单位rad *@input[in] W 目标速度,单位rad/s *@input[in] K_P 位置环环kp *@input[in] K_W 速度环kp *@note 控制公式 : t = T + (设定位置 - 实际位置)*K_P + (设定速度 - 实际速度)*K_W */ uint8_t xx; void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos, float W,float K_P,float K_W) { // a pointer to target motor GO_Motorfield* motor; motor = GO_motor_info+id; //这个rev是干啥的 // send preious cmd if (rev==1){ HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer)); return; } // assign motor target goal to the buffer motor->motor_send_data.head[0]=0xFE; motor->motor_send_data.head[1]=0xEE; motor->motor_send_data.mode.id = motor->id & 0xF; motor->motor_send_data.mode.status = motor->mode & 0x7; motor->motor_send_data.mode.none = 0x0; motor->motor_send_data.comd.tor_des = T*256; motor->motor_send_data.comd.spd_des = (W*256*6.33f)/(6.28319f); motor->motor_send_data.comd.pos_des = (Pos*32768*6.33f)/(6.28319f); motor->motor_send_data.comd.k_pos = K_P*1280; motor->motor_send_data.comd.k_spd = K_W*1280; uint8_t* mode = (uint8_t *)&(motor->motor_send_data.mode); motor->buffer[2] = *mode; motor->buffer[3] = (motor->motor_send_data.comd.tor_des) & 0xFF; motor->buffer[4] = (motor->motor_send_data.comd.tor_des>>8) & 0xFF; motor->buffer[5] = (motor->motor_send_data.comd.spd_des) & 0xFF; motor->buffer[6] = (motor->motor_send_data.comd.spd_des>>8) & 0xFF; motor->buffer[7] = (motor->motor_send_data.comd.pos_des) & 0xFF; motor->buffer[8] = (motor->motor_send_data.comd.pos_des>>8) & 0xFF; motor->buffer[9] = (motor->motor_send_data.comd.pos_des>>16) & 0xFF; motor->buffer[10] = (motor->motor_send_data.comd.pos_des>>24) & 0xFF; motor->buffer[11] = (motor->motor_send_data.comd.k_pos) & 0xFF; motor->buffer[12] = (motor->motor_send_data.comd.k_pos>>8) & 0xFF; motor->buffer[13] = (motor->motor_send_data.comd.k_spd) & 0xFF; motor->buffer[14] = (motor->motor_send_data.comd.k_spd>>8) & 0xFF; //crc calulation uint16_t crc = do_crc_table(motor->buffer, sizeof(motor->buffer)-2); motor->buffer[15] = (crc) & 0xFF; motor->buffer[16] = (crc>>8) & 0xFF; // interrupt buffer sending xx = HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer)); HAL_Delay(100); HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以 } //数据处理函数 void GO_M8010_recv_data(uint8_t* Temp_buffer) { // check for ID int8_t ID=-1; GO_Motorfield* motor; ID = Temp_buffer[2] & 0xF; // a pointer to target pointer motor = GO_motor_info + ID; memcpy(motor->Rec_buffer,Temp_buffer,16); // assign buffer to the target motor motor->mode = Temp_buffer[2]>>4 & 0xF; motor->correct = 1; motor->MError = 0; int16_t T = Temp_buffer[4]<<8 | Temp_buffer[3]; motor->T = T; motor->T /= 256; int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5]; motor->W = W; motor->W = (motor->W * 6.28319f )/(256*6.33f); int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7]; motor->Pos = Pos; motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f); int8_t Temp = Temp_buffer[11] & 0xFF; motor->Temp = Temp; motor->MError = Temp_buffer[12] & 0x7; } //力位混合控制 // for differnt robot should have different define. Now just for testing first /** *@brief 力位混合控制 *@input[in] huart 所用串口指针 *@input[in] id 被控电机id *@input[in] bias 前馈力矩 *@input[in] length 机械臂长度 *@input[in] mass 机械臂质量 *@input[in] tar_pos 目标位置 *@input[in] tar_w 目标速度 *@input[in] K_P 位置环kp *@input[in] K_W 速度环kp */ void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass, float tar_pos, float tar_w, float K_P,float K_W) { float forw_T = mass* gravit_const* length* (cos(GO_motor_info[id].Pos));//计算前馈力矩 forw_T += bias; GO_M8010_send_data(huart,id,0,forw_T,tar_pos,tar_w,K_P,K_W); } /** *@brief go发送控制命令 *@input[in] id 被控电机id *@revtal 返回电机数据指针 */ GO_Motorfield* getGoPoint(uint8_t id) { return &GO_motor_info[id]; }