To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 24:adb6bac314d7
- Parent:
- 23:47b8c7f9813e
- Child:
- 25:1aa10432cda2
diff -r 47b8c7f9813e -r adb6bac314d7 main.cpp --- a/main.cpp Wed Aug 07 12:54:09 2019 +0000 +++ b/main.cpp Tue Sep 03 16:29:39 2019 +0000 @@ -39,7 +39,8 @@ CAMERA1, CAMERA2, }JOINT; -Servo camera(PA_5); +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}; @@ -57,16 +58,16 @@ int present, current, torque, temperature; int status; -DigitalOut led(LED1); +DigitalOut led(PA_15); -communication_1 wire(PA_9, PA_10, BAUDRATE); +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(PB_5, PB_6); // RX, TX +CAN can1(PA_11, PA_12); // RX, TX CANMessage messageIn; CANMessage messageOut; @@ -91,7 +92,7 @@ can1.frequency(CAN_FREQUENCY); messageIn.format=CANExtended; messageOut.format=CANExtended; - + dyn_enable=1; wire.trigger(); wire.trigger(); wire.trigger(); @@ -132,9 +133,9 @@ wait(SLEEP); w_2.setMaxSpeed(dxl_speed[1]); wait(SLEEP); - w_2.setCWLimitUnits(1900); + //w_2.setCWLimitUnits(1900); wait(SLEEP); - w_2.setCCWLimitUnits(-1900); + //w_2.setCCWLimitUnits(-1900); wait(SLEEP); #if VERBOSE printf("DYNAMIXEL 2: SET MODE\n\r"); @@ -146,22 +147,24 @@ #if VERBOSE printf("DYNAMIXEL 3: SET MODE\n\r"); #endif - a_1.setMode(1); + /* + 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); + + //a_1.setGoal((dxl_goal_position[3]-dxl_offset[3])); + //wait(SLEEP); wait(1);