Team DIANA
FOrk
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 22:ffb26af4d5d8
- Parent:
- 21:43740448011a
--- a/main.cpp Fri Jun 21 07:42:44 2019 +0000
+++ b/main.cpp Mon Jul 29 22:34:34 2019 +0000
@@ -2,134 +2,346 @@
#include "communication_1.h"
#include "MX106.h"
#include "AX12.h"
-#include "Servo.h"
+#include <stdio.h>
+#include <string.h>
+
+
+#define VERBOSE 1
-#define SPEED 50
-
-#define JOINT_SET_SPEED 20
-
+#define CAN_FREQUENCY 125000
+#define BAUDRATE 57600
+#define SLEEP 0.02
+#define JOINT_SET_POSITION 20
+#define JOINT_STATUS 30
+#define JOINT_ERROR 40
#define JOINT1_ID 4
#define JOINT2_ID 5
#define JOINT3_ID 6
#define JOINT4_ID 7
-// Utility
-InterruptIn button(USER_BUTTON);
+
+int dxl_id[]={1,2,3,4};
+int joint_id[]={4,5,6,7};
+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};
+int present, current, torque, temperature;
+int status;
+
DigitalOut led(LED1);
-// Motor Control
-communication_1 wire(PA_9, PA_10, 57600);
-MX106 motor_1(wire, 1, 1);
-MX106 motor_2(wire, 2, 1);
-MX106 motor_3(wire, 3, 1);
-AX12 motor_4(wire, 4, 1);
+communication_1 wire(PA_9, PA_10, BAUDRATE);
-// Camera PanTilt Control
-//Servo cam1 (PC_0);
-//Servo cam2 (PC_1);
+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]);
-void button_int_handler()
-{
-
-}
-
-// CAN
-//Thread canrxa;
CAN can1(PB_5, PB_6); // RX, TX
CANMessage messageIn;
CANMessage messageOut;
-int pose;
-int current_pose[] = {0, 0, 0, 0, 0, 0};
+
-int main()
+int main ()
{
- can1.frequency(125000);
-
- messageIn.format=CANExtended;
-
- printf("CAN: Init DONE\n\r");
+ #if VERBOSE
+ printf("START \n\r");
+ #endif
+ can1.frequency(CAN_FREQUENCY);
+ messageIn.format=CANExtended;
+ messageOut.format=CANExtended;
+
+ 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
- wire.trigger();
- wire.trigger();
- wire.trigger();
- wire.trigger();
- wait(1);
-
- // Setup Motor1 MultiTurn
- motor_1.setMotorEnabled(1);
- motor_1.setMode(0);
- wait(3);
- printf("DYNAMIXEL: Init DONE 1\n\r");
+ 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(1);
+ 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);
- // Setup Motor2 MultiTurn
- motor_2.setMotorEnabled(1);
- motor_2.setMode(0);
- wait(3);
- printf("DYNAMIXEL: Init DONE 2\n\r");
+ while (1)
+ {
+ if(can1.read(messageIn))
+ {
+ #if VERBOSE
+ printf("CAN: Message passed!\tId: %d\n\r", messageIn.id);
+ #endif
+ led!=led;
+ if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT1_ID))
+ {
+ dxl_goal_position[0] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+ }
+ if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT2_ID))
+ {
+ dxl_goal_position[1] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+ }
+ if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT3_ID))
+ {
+ dxl_goal_position[2] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+ }
+ if(messageIn.id == ((JOINT_SET_POSITION << 8) + JOINT4_ID))
+ {
+ dxl_goal_position[3] = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
+ }
- // Setup Motor3 MultiTurn
- motor_3.setMotorEnabled(1);
- motor_3.setMode(0);
- wait(3);
- printf("DYNAMIXEL: Init DONE 3\n\r");
+ if(messageIn.id == ((JOINT_ERROR << 8) + JOINT1_ID))
+ {
+ dxl_reset[0] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+ }
+ if(messageIn.id == ((JOINT_ERROR << 8) + JOINT2_ID))
+ {
+ dxl_reset[1] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+ }
+ if(messageIn.id == ((JOINT_ERROR << 8) + JOINT3_ID))
+ {
+ dxl_reset[2] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+ }
+ if(messageIn.id == ((JOINT_ERROR << 8) + JOINT4_ID))
+ {
+ dxl_reset[3] = ((messageIn.data[2] << 8) + (messageIn.data[3]));
+ }
+ }
- // Setup Motor4 MultiTurn
- motor_4.setMotorEnabled(1);
- motor_4.setMode(0);
- wait(3);
- printf("DYNAMIXEL: Init DONE 4\n\r");
+ 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);
- printf("Running!\n\r");
-
- int32_t speed = 0;
+ 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);
- while(true)
- {
- if(can1.read(messageIn))
- {
- if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT1_ID))
- {
- speed = 0;
- speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-
- motor_1.setSpeed(speed);
-
- printf("CAN: mess %d\n\r", speed);
- }
-
- if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT2_ID))
- {
- speed = 0;
- speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-
- motor_2.setSpeed(speed);
-
- printf("CAN: mess %d\n\r", speed);
- }
-
- if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT3_ID))
- {
- speed = 0;
- speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-
- motor_3.setSpeed(speed);
-
- printf("CAN: mess %d\n\r", speed);
- }
-
- if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT4_ID))
- {
- speed = 0;
- speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
-
- motor_4.setSpeed(speed);
-
- printf("CAN: mess %d\n\r", speed);
- }
+ #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 = 8;
+ 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 = (JOINT_STATUS << 8) + joint_id[i];
+
+ messageOut.data[0] = (present >> 8);
+ messageOut.data[1] = (present);
+ messageOut.data[2] = (current >> 8);
+ messageOut.data[3] = (current);
+ messageOut.data[4] = (torque >> 8);
+ messageOut.data[5] = (torque);
+ messageOut.data[6] = (temperature >> 8);
+ messageOut.data[7] = (temperature);
+
+ status = can1.write(messageOut);
+
+ #if VERBOSE
+ printf("CAN send status Joint %d : %d", joint_id[i] , 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= ( JOINT_ERROR << 8 ) + joint_id[i];
+
+ messageOut.data[0]=(((int)(dxl_max_torque[i]))>>8);
+ messageOut.data[1]= ((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
+ }
+
+
}
-
- wait(0.1);
- }
-}
\ No newline at end of file
+ return 0;
+}