Gen3_Dropper

CommDropper.ino

@@ -41,8 +41,8 @@ const bool RADIO_NOT_PRESENT = HIGH;
 const unsigned int NUMBER_OF_RADIOS = 6;
 const int NUMBER_OF_DROPPERS = 6;
 const unsigned int ALLOWABLE_POSITION_ERROR = 50;
-const unsigned int HARDSTOP_OFFSET = 25;
-const unsigned int REST_POSITION_OFFSET = 300;
+const unsigned int HARDSTOP_OFFSET = 400;
+const unsigned int REST_POSITION_OFFSET = 750;
 const unsigned int PIN_RELEASE = 9;
 const unsigned int DELAY_T_RELEASE = 1000UL;
 const unsigned int DELAY_T_STAGE = 1000UL;
@@ -52,8 +52,8 @@ const unsigned int RELEASE_POSITION_CLOSE = 512;
 const unsigned int NOMINAL_SPEED = 250;  //How fast to  move up and down
 const unsigned int CALIBRATION_SPEED = 50;  //How fast to  move up and down
 const unsigned int SERVO_MOVE_TIMEOUT = 8UL * 1000;
-const unsigned int INITIAL_TOP_POS = -8000;
-const unsigned int INITIAL_BOTTOM_POS = 8000;
+const unsigned int INITIAL_TOP_POS = 1000;
+const unsigned int INITIAL_BOTTOM_POS = 7000;
 #pragma endregion
 
 #pragma region STRUCTS

State_FMS.ino

@@ -105,7 +105,7 @@ void Update_Top_Cal() {
 	if (Top_Hardstop.isPressed()) {
 		//has reached bottom hardstop, set this as bottom position
 		nh.loginfo("Top hard stop pressed");
-		servo_pos_msg.data = Dynamixel.readPosition(BIG_SERVO.ID);
+		servo_pos_msg.data = BIG_SERVO.position_deg;
 		BIG_SERVO.top_position = Add_offset_top(BIG_SERVO.position_deg, HARDSTOP_OFFSET); // Sign?
 		BIG_SERVO.goal_position_deg = BIG_SERVO.top_position;
 		Dynamixel.moveSpeed(BIG_SERVO.ID, BIG_SERVO.goal_position_deg, BIG_SERVO.goal_speed); // Asks the servo to stay in position
@@ -132,7 +132,7 @@ void Update_Bottom_Cal() {
 	if (Bottom_Hardstop.isPressed()) {
 		//has reached bottom hardstop, set this as bottom position
 		nh.loginfo("Bottom hard stop pressed");
-		servo_pos_msg.data = Dynamixel.readPosition(BIG_SERVO.ID);
+		servo_pos_msg.data = BIG_SERVO.position_deg;
 		BIG_SERVO.bottom_position = Add_offset_bottom(BIG_SERVO.position_deg , HARDSTOP_OFFSET ); // Sign?
 		BIG_SERVO.goal_position_deg = BIG_SERVO.bottom_position;
 		Dynamixel.moveSpeed(BIG_SERVO.ID, BIG_SERVO.goal_position_deg, BIG_SERVO.goal_speed);
@@ -141,11 +141,10 @@ void Update_Bottom_Cal() {
 }
 
 void Exit_Bottom_Cal() {
-	DROPPER.Is_Calibrated = true;
-	BIG_SERVO.rest_position = Add_offset_top(BIG_SERVO.top_position, HARDSTOP_OFFSET);
+	BIG_SERVO.rest_position = Add_offset_top(BIG_SERVO.top_position, REST_POSITION_OFFSET);
 	Dynamixel.moveSpeed(BIG_SERVO.ID, BIG_SERVO.rest_position, BIG_SERVO.goal_speed);
-	//command.cal_cmd = false;
 	nh.loginfo("Calibration Finished!");
+	DROPPER.Is_Calibrated = true;
 }
 #pragma endregion
 
@@ -205,6 +204,7 @@ void Update_Start_Load() {
 		{
 			SMALL_SERVO[i].goal_position_deg = SMALL_SERVO[i].open_position;
 			Dynamixel.move(SMALL_SERVO[i].ID, SMALL_SERVO[i].goal_position_deg);
+			delay(100);
 		}
 		FSM_stateMachine.transitionTo(FSM_state_Wait_For_Load);
 	}
@@ -228,6 +228,7 @@ void Update_Wait_For_Load() {// Togle load command or press the button for confi
 		{
 			SMALL_SERVO[i].goal_position_deg = SMALL_SERVO[i].close_position;
 			Dynamixel.move(SMALL_SERVO[i].ID, SMALL_SERVO[i].goal_position_deg);
+			delay(100);
 		}
 		FSM_stateMachine.transitionTo(FSM_state_Finish_Load);
 	}