Diff: main.cpp

Revision:

24:adb6bac314d7

Parent:

23:47b8c7f9813e

Child:

25:1aa10432cda2

--- 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);