Commit 4a71e87d authored by Alessandro Buscicchio's avatar Alessandro Buscicchio

Gen3_Dropper

parent ec9a7043
......@@ -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
......
......@@ -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);
}
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment