To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
main.cpp
- Committer:
- gidiana
- Date:
- 2019-09-03
- Revision:
- 24:adb6bac314d7
- Parent:
- 23:47b8c7f9813e
- Child:
- 25:1aa10432cda2
File content as of revision 24:adb6bac314d7:
#include "mbed.h" #include "communication_1.h" #include "MX106.h" #include "AX12.h" #include <stdio.h> #include <string.h> #include "Servo.h" #define VERBOSE 1 #define CAN_FREQUENCY 125000 #define BAUDRATE 57600 #define SLEEP 0.02 typedef enum { JOINT_SET_SPEED = 20, JOINT_SET_POSITION, JOINT_CURRENT_POSITION, JOINT_CURRENT_SPEED, JOINT_STATUS, JOINT_ERROR, JOINT_TORQUE, JOINT_MAXTORQUE, JOINT_ZERO, }CAN_COMMANDS; typedef enum { BASE=1, SHOULDER, ELBOW, WRIST1, WRIST2, WRIST3, END_EFFECTOR, CAMERA1, CAMERA2, }JOINT; Servo camera(PA_8); DigitalOut dyn_enable(PC_11); int dxl_id[]={1,2,3,4}; JOINT joint_id[]={WRIST1,WRIST2,WRIST3,CAMERA1,CAMERA2}; int dxl_speed[]={50,25,50,50}; float dxl_gear[]={3,2,3,1}; float dxl_present_position[] = {0,0,0,0}; float dxl_goal_position[] = {0,0,0,0}; float dxl_offset[] = {0,0,0,0}; float dxl_current[] = {0,0,0,0}; float dxl_torque[] = {0,0,0,0}; float dxl_max_torque[] = {0,0,0,0}; float dxl_reset[]={0,0,0,0}; uint16_t dxl_temperature[]={0,0,0,0}; uint16_t dxl_limit[]={0,0,0,0}; float camera_pose=0; int present, current, torque, temperature; int status; DigitalOut led(PA_15); communication_1 wire(PB_10, PC_5, BAUDRATE); MX106 w_1(wire, dxl_id[0], dxl_gear[0]); MX106 w_2(wire, dxl_id[1], dxl_gear[1]); MX106 w_3(wire, dxl_id[2], dxl_gear[2]); AX12 a_1(wire, dxl_id[3], dxl_gear[3]); CAN can1(PA_11, PA_12); // RX, TX CANMessage messageIn; CANMessage messageOut; uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id) { uint32_t id = (uint32_t)can_id; // LSB byte is the controller id. id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id. id |= 0x80000000; // Send in Extended Frame Format. return id; } int main () { #if VERBOSE printf("START \n\r"); #endif can1.frequency(CAN_FREQUENCY); messageIn.format=CANExtended; messageOut.format=CANExtended; dyn_enable=1; wire.trigger(); wire.trigger(); wire.trigger(); wire.trigger(); #if VERBOSE printf("DYNAMIXEL: Init START \n\r"); #endif w_1.setMotorEnabled(1); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL: Init 1 \n\r"); #endif w_2.setMotorEnabled(1); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL: Init 2 \n\r"); #endif w_3.setMotorEnabled(1); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL: Init 3\n\r"); #endif a_1.setMotorEnabled(1); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL: Init 4\n\r"); #endif w_1.setMode(2); wait(SLEEP); w_1.setMaxSpeed(dxl_speed[0]); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL 1: SET MODE\n\r"); #endif w_2.setMode(2); wait(SLEEP); w_2.setMaxSpeed(dxl_speed[1]); wait(SLEEP); //w_2.setCWLimitUnits(1900); wait(SLEEP); //w_2.setCCWLimitUnits(-1900); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL 2: SET MODE\n\r"); #endif w_3.setMode(2); wait(SLEEP); w_3.setMaxSpeed(dxl_speed[2]); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL 3: SET MODE\n\r"); #endif /* a_1.setMode(2); wait(SLEEP); a_1.setMaxSpeed(dxl_speed[3]); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL 4: SET MODE\n\r"); #endif */ w_1.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); w_2.setGoal((dxl_goal_position[1]-dxl_offset[1])); wait(SLEEP); w_3.setGoal((dxl_goal_position[2]-dxl_offset[2])); wait(SLEEP); //a_1.setGoal((dxl_goal_position[3]-dxl_offset[3])); //wait(SLEEP); wait(1); while (1) { if(can1.read(messageIn)) { #if VERBOSE printf("CAN: Message passed!\tId: %d\n\r", messageIn.id); #endif led!=led; if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST1)) { dxl_goal_position[0] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); } if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST2)) { dxl_goal_position[1] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); } if(messageIn.id == gen_can_id(JOINT_SET_POSITION, WRIST3)) { dxl_goal_position[2] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); } if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA1)) { dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); } if(messageIn.id == gen_can_id(JOINT_SET_POSITION, CAMERA2)) { camera_pose = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3])/100; } if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST1)) { dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3])); } if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST2)) { dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3])); } if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, WRIST3)) { dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3])); } if(messageIn.id == gen_can_id(JOINT_MAXTORQUE, CAMERA1)) { dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3])); } if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST1)) { if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) { w_1.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); } } if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST2)) { if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) { w_2.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); } } if(messageIn.id == gen_can_id(JOINT_ZERO, WRIST3)) { if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) { w_3.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); } } if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA1)) { if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) { a_1.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); } } if(messageIn.id == gen_can_id(JOINT_ZERO, CAMERA2)) { if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1) { camera=0; } } } w_1.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); w_2.setGoal((dxl_goal_position[1]-dxl_offset[1])); wait(SLEEP); w_3.setGoal((dxl_goal_position[2]-dxl_offset[2])); wait(SLEEP); a_1.setGoal((dxl_goal_position[3]-dxl_offset[3])); wait(SLEEP); camera=camera_pose; dxl_present_position[0]=w_1.getPosition(); wait(SLEEP); dxl_present_position[1]=w_2.getPosition(); wait(SLEEP); dxl_present_position[2]=w_3.getPosition(); wait(SLEEP); dxl_present_position[3]=a_1.getPosition(); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL PRESENT POSITION %f %f %f %f\n\r", dxl_present_position[0],dxl_present_position[1], dxl_present_position[2] ,dxl_present_position[3] ); #endif dxl_current[0]=w_1.getCurrent(); wait(SLEEP); dxl_current[1]=w_2.getCurrent(); wait(SLEEP); dxl_current[2]=w_3.getCurrent(); wait(SLEEP); //dxl_current[3]=a_1.getCurrent(); //wait(SLEEP); #if VERBOSE printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] ); #endif dxl_torque[0]=dxl_current[0]*0.875; dxl_torque[1]=dxl_current[1]*0.875; dxl_torque[2]=dxl_current[2]*0.875; //dxl_torque[3]=dxl_current[3]*0.875; #if VERBOSE printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] ); #endif dxl_temperature[0]=w_1.getTemp(); wait(SLEEP); dxl_temperature[1]=w_2.getTemp(); wait(SLEEP); dxl_temperature[2]=w_3.getTemp(); wait(SLEEP); dxl_temperature[3]=a_1.getTemp(); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL TEMPERATURE %d %d %d %d\n\r", dxl_temperature[0],dxl_temperature[1], dxl_temperature[2] ,dxl_temperature[3] ); #endif for(int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++) { messageOut.len = 4; present=(int)(dxl_present_position[i]*100); current=(int)(dxl_current[i]*100); torque=(int)(dxl_torque[i]*100); temperature=(int)(dxl_temperature[i]*100); messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, joint_id[i]); messageOut.data[0] = (present >> 24); messageOut.data[1] = (present>>16); messageOut.data[2] = (present>>8); messageOut.data[3] = (present); status = can1.write(messageOut); #if VERBOSE printf("CAN send present position Joint %d : %d", joint_id[i] , status ); #endif messageOut.id = gen_can_id(JOINT_STATUS, joint_id[i]); messageOut.data[0] = (current >> 8); messageOut.data[1] = (current); messageOut.data[2] = (temperature >> 8); messageOut.data[3] = (temperature); status = can1.write(messageOut); #if VERBOSE printf("CAN send status Joint %d : %d", joint_id[i] , status ); #endif messageOut.id = gen_can_id(JOINT_TORQUE, joint_id[i]); messageOut.data[0] = (torque >> 24); messageOut.data[1] = (torque >> 16); messageOut.data[2] = (torque >> 8); messageOut.data[3] = (torque); status = can1.write(messageOut); #if VERBOSE printf("CAN send status Joint %d : %d", joint_id[i] , status ); #endif } int _camera_pose = camera_pose*100; messageOut.len = 4; messageOut.id = gen_can_id(JOINT_CURRENT_POSITION, CAMERA2); messageOut.data[0] = (_camera_pose >> 24); messageOut.data[1] = (_camera_pose >> 16); messageOut.data[2] = (_camera_pose >> 8); messageOut.data[3] = (_camera_pose); status = can1.write(messageOut); #if VERBOSE printf("CAN send present position Joint %d : %d", CAMERA2 , status ); #endif dxl_max_torque[0]=w_1.getTorqueMax(); wait(SLEEP); dxl_max_torque[1]=w_2.getTorqueMax(); wait(SLEEP); dxl_max_torque[2]=w_3.getTorqueMax(); wait(SLEEP); dxl_max_torque[3]=a_1.getTorqueMax(); wait(SLEEP); for (int i=0; i<( sizeof(dxl_id) / sizeof(dxl_id[0])); i++ ) { messageOut.len = 4; messageOut.id= gen_can_id(JOINT_ERROR, joint_id[i]); messageOut.data[0]= ((int)(dxl_max_torque[i]) >> 24); messageOut.data[1]= ((int)(dxl_max_torque[i]) >> 16); messageOut.data[2]= ((int)(dxl_max_torque[i]) >> 8); messageOut.data[3]= ((int)(dxl_max_torque[i]) ); status = can1.write(messageOut); #if VERBOSE printf("CAN send status ERROR Joint %d : %d", joint_id[i] , status ); #endif } if(dxl_reset[0]!=0) { w_1.setMotorEnabled(0); wait(SLEEP); w_1.setMaxTorque(1); wait(SLEEP); w_1.setMotorEnabled(1); wait(SLEEP); w_1.setGoal(0-dxl_offset[0]); wait(SLEEP); #if VERBOSE printf("Dynamixel %d : RESET", joint_id[0] ); #endif } if(dxl_reset[1]!=0) { w_2.setMotorEnabled(0); wait(SLEEP); w_2.setMaxTorque(1); wait(SLEEP); w_2.setMotorEnabled(1); wait(SLEEP); w_2.setGoal(0-dxl_offset[1]); wait(SLEEP); #if VERBOSE printf("Dynamixel %d : RESET", joint_id[1] ); #endif } if(dxl_reset[2]!=0) { w_3.setMotorEnabled(0); wait(SLEEP); w_3.setMaxTorque(1); wait(SLEEP); w_3.setMotorEnabled(1); wait(SLEEP); w_3.setGoal(0-dxl_offset[2]); wait(SLEEP); #if VERBOSE printf("Dynamixel %d : RESET", joint_id[2] ); #endif } /* if(dxl_reset[3]!=0) { a_1.setMotorEnabled(0); wait(SLEEP); a_1.setMaxTorque(1); wait(SLEEP); a_1.setMotorEnabled(1); wait(SLEEP); a_1.setGoal(0-dxl_offset[3]); wait(SLEEP); #if VERBOSE printf("Dynamixel %d : RESET", joint_id[3] ); #endif } */ } }