Browse Source

Fix init and hinge closing mode

Init and hinge closing mode were completely wrong.
master
Marc Poulhiès 2 years ago
parent
commit
cbd0cde9f3
  1. 3
      arduino/config.h
  2. 59
      arduino/startracker.ino

3
arduino/config.h

@ -21,6 +21,9 @@ static const float axis_hinge_dist_mm = 200;
/* Initial deployment of threaded rod in millimeters */
static const float initial_rod_deploy = 30;
/* Smallest value for hinge. Going lower would damage the gears/motors */
static const float min_rod_deploy = 30;
/* millimeters per rotation of the threaded-rod */
static const float bolt_thread_mm = 1.25;

59
arduino/startracker.ino

@ -72,15 +72,32 @@ static const float stepper_gear_rad_per_step = (2*PI) / nr_steps;
static unsigned int loop_count = 0;
#endif
// this needs to be reset
static struct rot_state_t {
struct rot_state_t {
unsigned long elapsed_time_millis;
float stepper_gear_rot_rad = 0;
} rot_state;
};
static float get_expected_stepper_rot(rot_state_t *s);
// The init function simply skips time from the 0 (hinge is closed) to
// the initial state configured by the initial_rod_deploy value. When
// hinge is opening, it's really simply skipping time. When hinge is
// closing, there is a change in reference for the angle.
//
// If hinge opening mode, angle here is simply the angle formed by the
// moving plate and the fixed plate, constrained by the initial rod
// deployment.
//
// In hinge closing mode, angle is computed the same way, but the
// reference is still 0 with the earth rotation growing, so we take
// the complementary (PI - a).
static void init_rot_state(struct rot_state_t *state) {
rot_state.stepper_gear_rot_rad = 0;
rot_state.elapsed_time_millis = atan(initial_rod_deploy / axis_hinge_dist_mm);
if (hinge_opening)
state->elapsed_time_millis = atan(initial_rod_deploy / axis_hinge_dist_mm) / earth_rot_speed_rad_msec;
else
state->elapsed_time_millis = (PI - atan(initial_rod_deploy / axis_hinge_dist_mm)) / earth_rot_speed_rad_msec;
state->stepper_gear_rot_rad = get_expected_stepper_rot(state);
}
static const unsigned int btn1_pin = 8;
@ -131,16 +148,15 @@ static void debug_long(rot_state_t *s){
}
#endif
// Returns the value in radian the motor should have turned to reach
// the current value of earth rotation since the beggining.
static float
get_expected_stepper_rot(rot_state_t *s)
{
float a = earth_rot_speed_rad_msec * s->elapsed_time_millis /* ellapsed_in_sec */;
if (!hinge_opening)
a = 1.57079632679 /* PI/2 */ - a;
if (hinge_opening)
a = PI - a;
const float r = tan(a)
* axis_hinge_dist_mm
@ -165,16 +181,27 @@ get_expected_stepper_rot(rot_state_t *s)
return r;
}
// Returns the length of the rod really deployed.
static float
get_rod_deploy (rot_state_t *s)
{
float a = s->elapsed_time_millis * earth_rot_speed_rad_msec;
if (!hinge_opening)
a = PI - a;
return tan(a) * axis_hinge_dist_mm;
}
// Returns the number of steps and the direction needed to reach given
// rotation angle in radian.
static int
get_step_number(rot_state_t *s, float expected_rotation)
{
const float angle_diff = expected_rotation - s->stepper_gear_rot_rad;
const float fsteps = angle_diff / stepper_gear_rad_per_step;
const int steps = floor(fsteps);
#if DEBUG
Serial.print("current rot:");
Serial.println(s->stepper_gear_rot_rad, 6);
@ -292,7 +319,7 @@ static bool new_state = true;
new_state = true; \
control_state = name; \
} while(0)
#else
#define STATE(name)
#define NEXT_STATE(name) \
@ -300,6 +327,9 @@ static bool new_state = true;
#endif
// this needs to be reset
static struct rot_state_t rot_state;
static void
control_automata(void)
{
@ -356,6 +386,11 @@ control_automata(void)
stop_timer();
stepper.disable();
NEXT_STATE(IDLE);
} else if (get_rod_deploy(&rot_state) < min_rod_deploy) {
dwprint("Min value for hinge reached, RUN => IDLE");
stop_timer();
stepper.disable();
NEXT_STATE(IDLE);
} else if (active_timer.expired) {
active_timer.expired--;
// emit_motor_step();

Loading…
Cancel
Save