Skip to content

new drop routine, tested on hardware #52

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
graysonk546 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

new drop routine, tested on hardware #52

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
66 changes: 59 additions & 7 deletions fw/src/app/depositor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,17 @@
* C O N S T A N T S *
*******************************************************************************/

#define DEPOSITOR_CLOSE_STEPS 255
#define DEPOSITOR_CLOSE_RATE 255
#define DEPOSITOR_CLOSE_ANGLE 62.0 // Fully closed

#define DEPOSITOR_OPEN_STEPS 255
#define DEPOSITOR_OPEN_RATE 25
#define DEPOSITOR_OPEN_ANGLE 0.0 //-90.0 achieves full open

#define DEPOSITOR_CLOSE_STEPS 255
#define DEPOSITOR_CLOSE_RATE 255
#define DEPOSITOR_CLOSE_ANGLE 62.0 // Fully closed
#define DEPOSITOR_SLIT_ANGLE DEPOSITOR_CLOSE_ANGLE - 12.0
#define DEPOSITOR_SLIT_RATE 255
#define DEPOSITOR_SLIT_STEPS 5

#define DEPOSITOR_ARM_HOME_ANGLE 0.0
#define DEPOSITOR_ARM_CENTER_ANGLE 75.0
Expand All @@ -31,6 +35,8 @@

#define DEPOSITOR_ARM_CW 1

#define DEPOSITOR_JOSTLE_MOVEMENTS 16

/*******************************************************************************
* D A T A D E C L A R A T I O N S *
*******************************************************************************/
Expand All @@ -39,6 +45,8 @@ typedef struct
{
struct pt thread;
depositor_state_E state;
int8_t jostle_counter;
float jostle_routine[DEPOSITOR_JOSTLE_MOVEMENTS];
} depositor_data_S;

/*******************************************************************************
Expand All @@ -56,6 +64,8 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state);
static depositor_data_S depositor_data =
{
.state = DEPOSITOR_STATE_HOMING,
.jostle_counter = DEPOSITOR_JOSTLE_MOVEMENTS - 1,
.jostle_routine = {-1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1},
};

/*******************************************************************************
Expand Down Expand Up @@ -121,11 +131,11 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state)
}
else
{
next_state = DEPOSITOR_STATE_DROPPING;
next_state = DEPOSITOR_STATE_CENTERING;
}
break;

case DEPOSITOR_STATE_DROPPING:
case DEPOSITOR_STATE_CENTERING:
if (stepper_commandAngle(STEPPER_DEPOSITOR,
DEPOSITOR_ARM_CENTER_ANGLE,
DEPOSITOR_ARM_RAMP_ANGLE,
Expand All @@ -135,16 +145,58 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state)
// do nothing, navigating to the center
}
else
{
next_state = DEPOSITOR_STATE_DROPPING;
// if (servo_command(SERVO_DEPOSITOR,
// DEPOSITOR_SLIT_ANGLE,
// DEPOSITOR_SLIT_STEPS,
// DEPOSITOR_SLIT_RATE) == false)
// {
// // do nothing
// }
// else
// {
// next_state = DEPOSITOR_STATE_DROPPING;
// }
}
break;

case DEPOSITOR_STATE_DROPPING:
if (depositor_data.jostle_counter >= 0)
{
uint8_t j = depositor_data.jostle_counter;
float dtheta = depositor_data.jostle_routine[j];
if (stepper_commandAngle(STEPPER_DEPOSITOR,
DEPOSITOR_ARM_CENTER_ANGLE + dtheta,
0.0,
DEPOSITOR_ARM_NAV_RATE,
DEPOSITOR_ARM_NAV_RATE) == false ||
servo_command(SERVO_DEPOSITOR,
DEPOSITOR_SLIT_ANGLE - (0.1*(DEPOSITOR_JOSTLE_MOVEMENTS-j)),
DEPOSITOR_SLIT_STEPS,
DEPOSITOR_OPEN_RATE) == false)
{
// wait for current motion to finish
}
else
{
depositor_data.jostle_counter--;
}
}
else
{
if (servo_command(SERVO_DEPOSITOR,
DEPOSITOR_OPEN_ANGLE,
DEPOSITOR_OPEN_STEPS,
1,
DEPOSITOR_OPEN_RATE) == false)
{
// do nothing
// wait for cup to open
}
else
{
// reset jostle counter for next time, change state
depositor_data.jostle_counter = DEPOSITOR_JOSTLE_MOVEMENTS \
- 1;
next_state = DEPOSITOR_STATE_ENTERING_IDLE;
}
}
Expand Down