From ac5971ce3952b84f9333ee4847482fb10c1e29de Mon Sep 17 00:00:00 2001
From: Jake <jake.read@cba.mit.edu>
Date: Tue, 1 May 2018 11:04:20 -0400
Subject: [PATCH] does single blocks
---
.../mkstepper-v011/apahandler.c | 14 ++-
.../mkstepper-v011/mkstepper-v011/stepper.c | 113 +++++++++++++-----
.../mkstepper-v011/mkstepper-v011/stepper.h | 27 +++--
3 files changed, 112 insertions(+), 42 deletions(-)
diff --git a/embedded/mkstepper-v011/mkstepper-v011/apahandler.c b/embedded/mkstepper-v011/mkstepper-v011/apahandler.c
index 34ea035..9be5179 100644
--- a/embedded/mkstepper-v011/mkstepper-v011/apahandler.c
+++ b/embedded/mkstepper-v011/mkstepper-v011/apahandler.c
@@ -51,8 +51,18 @@ void apa_handle_packet(uint8_t *packet, uint8_t length){
if(i + 12 > length){ // confirm: not i + 12 >= ?
i ++; // avoid hangup
} else {
- // in steps / s
- //stepper_segment(&stepper, startspeed, accel, steps);
+ // in steps (discrete)
+ int32_t steps = (packet[i+1] << 24) | (packet[i+2] << 16) | (packet[i+3] << 8) | packet[i+4];
+ // in steps/s
+ uint32_t entryspeed = (packet[i+5] << 24) | (packet[i+6] << 16) | (packet[i+7] << 8) | packet[i+8];
+ // in steps/s/s
+ uint32_t accel = (packet[i+9] << 24) | (packet[i+10] << 16) | (packet[i+11] << 8) | packet[i+12];
+ // in steps/s
+ uint32_t accellength = (packet[i+13] << 24) | (packet[i+14] << 16) | (packet[i+15] << 8) | packet[i+16];
+ // in steps/s
+ uint32_t deccellength = (packet[i+17] << 24) | (packet[i+18] << 16) | (packet[i+19] << 8) | packet[i+20];
+ // do the business
+ stepper_block(&stepper, steps, entryspeed, accel, accellength, deccellength);
i += 13; // ? not 12 ?
}
break;
diff --git a/embedded/mkstepper-v011/mkstepper-v011/stepper.c b/embedded/mkstepper-v011/mkstepper-v011/stepper.c
index a9b8976..cacd18e 100644
--- a/embedded/mkstepper-v011/mkstepper-v011/stepper.c
+++ b/embedded/mkstepper-v011/mkstepper-v011/stepper.c
@@ -13,64 +13,117 @@ stepper_t stepper_new(pin_t *step_pin, pin_t *dir_pin){
stepper.step_pin = step_pin;
stepper.dir_pin = dir_pin;
-
- stepper.speed_period = 0;
- stepper.position_ticks = 0;
- stepper.position_ticks_target = 0;
+ stepper_reset(&stepper);
return stepper;
}
void stepper_reset(stepper_t *stepper){
+
+ stepper->one_minute = 160980000; // calib'd # for timer ticks per minute (speeds, accels all in steps/min or steps/min/s)
stepper->speed_period = 0;
+ stepper->accel_period = 0;
+
+ stepper->speed = 0;
+
+ stepper->last_step = 0;
+ stepper->last_accel = 0;
+
stepper->position_ticks = 0;
- stepper->position_ticks_target = 0;
- stepper->dir_steps = 1;
+ stepper->position_accel_to = 0;
+ stepper->position_deccel_from = 0;
+ stepper->position_ticks_end = 0;
}
void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed){
+ // set speed period
+ (speed < 1) ? speed = 1 : (0); // avoid 0 division, 1 step / s seems like reasonable lower bound step rate
+ stepper->speed = speed;
+ stepper->speed_period = stepper->one_minute / stepper->speed;
+
+ // set direction
+ if(steps < 0){
+ pin_clear(stepper->dir_pin);
+ } else {
+ pin_set(stepper->dir_pin);
+ }
+
// set new position information
- stepper->position_ticks_target = abs(steps);
+ stepper->position_ticks_end = abs(steps);
+ stepper->position_accel_to = 0;
+ stepper->position_deccel_from = stepper->position_ticks_end; // defines flat line block
stepper->position_ticks = 0;
- // set direction
+ // reset times
+ TICKER_SYNC;
+ unsigned long now = TICKER;
+ stepper->last_accel = now;
+ stepper->last_step = now;
+}
+
+void stepper_block(stepper_t *stepper, int32_t steps, uint32_t entryspeed, uint32_t accel, uint32_t accellength, uint32_t deccellength){
+ // or, newblock: does maths and adds to queue
+
+ // enforce no div/0
+ (entryspeed < 1) ? entryspeed = 1 : (0);
+ // going to have to catch blocks which cause deceleration to 0 during deceleration phases !
+ stepper->speed = entryspeed;
+
+ // do starting speed period
+ stepper->speed_period = stepper->one_minute / stepper->speed;
+ stepper->accel_period = stepper->one_minute / accel;
+
+ // set dir
if(steps < 0){
- stepper->dir_steps = 0;
pin_clear(stepper->dir_pin);
} else {
- stepper->dir_steps = 1;
pin_set(stepper->dir_pin);
}
- // set speed period
- (speed < 1) ? speed = 1 : (0); // avoid 0 division, 1 step / s seems like reasonable lower bound step rate
- stepper->speed = speed;
- stepper->speed_period = 2683000 / speed;
+ // do lengths
+ stepper->position_ticks_end = abs(steps);
+ stepper->position_accel_to = accellength;
+ stepper->position_deccel_from = deccellength;
+ stepper->position_ticks = 0;
+
+ // reset times
+ TICKER_SYNC;
+ unsigned long now = TICKER;
+ stepper->last_accel = now;
+ stepper->last_step = now;
}
-void stepper_update(stepper_t *stepper){
- if(stepper->position_ticks_target > stepper->position_ticks){ // still have somewhere to go
+void stepper_update(stepper_t *stepper){
+ if(stepper->position_ticks_end > stepper->position_ticks){ // still have somewhere to go
TICKER_SYNC;
- uint32_t now = TICKER;
- // check if it's time to accel
- /*
- if(now - stepper->last_accel > stepper->accel_period){
- if(stepper->dir_accel){
- uint32_t newspeed = stepper->speed + 1;
- stepper_speed(stepper, newspeed);
- } else {
- uint32_t newspeed = stepper->speed - 1;
- stepper_speed(stepper, newspeed);
+ unsigned long now = TICKER;
+
+ if(stepper->position_ticks < stepper->position_accel_to){
+ // we're accelerating!
+ if(now - stepper->last_accel > stepper->accel_period){
+ stepper->speed += 1;
+ stepper->speed_period = stepper->one_minute / stepper->speed;
+ stepper->last_accel = now;
+ }
+ } else if(stepper->position_ticks > stepper->position_deccel_from){
+ if(now - stepper->last_accel > stepper->accel_period){
+ stepper->speed -= 1;
+ (stepper->speed < 1) ? stepper->speed = 1 : (0); // assert no 0's
+ stepper->speed_period = stepper->one_minute / stepper->speed;
+ stepper->last_accel = now;
}
- stepper->last_accel = now;
+ } else {
+ // no accel or deccel
}
- */
+
// check if it's time to step
- if(now - stepper->last_step > stepper->speed_period){
+ if(now - stepper->last_step >= stepper->speed_period){
pin_toggle(stepper->step_pin);
stepper->position_ticks ++;
- stepper->last_step = now;
+ stepper->last_step = now; //stepper->last_step + stepper->speed_period; // last speed_period if accelerating
+ // not sure why that wasn't working: for now, take for granted that over the course of many steps,
+ // we tend do equal amounts undershooting speed on all motors
} // end step cycle
}// end step if not there cycle
diff --git a/embedded/mkstepper-v011/mkstepper-v011/stepper.h b/embedded/mkstepper-v011/mkstepper-v011/stepper.h
index ea55413..e40f3e2 100644
--- a/embedded/mkstepper-v011/mkstepper-v011/stepper.h
+++ b/embedded/mkstepper-v011/mkstepper-v011/stepper.h
@@ -16,28 +16,35 @@ typedef struct {
pin_t *step_pin;
pin_t *dir_pin;
- // currently.. why do we need so much state?
- uint32_t speed_period;
- uint32_t speed;
- uint32_t accel_period;
- uint32_t position_ticks;
- uint32_t dir_steps; // 0 or 1
- uint32_t dir_accel;
+ // tracking time periods
+ unsigned long one_minute;
+ unsigned long speed_period;
+ unsigned long accel_period;
// tracking time for updates
- uint32_t last_step;
- uint32_t last_accel;
+ unsigned long last_step;
+ unsigned long last_accel;
+
+ // have to track speed to update accel
+ uint32_t speed;
// targets
- uint32_t position_ticks_target;
+ uint32_t position_ticks;
+ uint32_t position_accel_to;
+ uint32_t position_deccel_from;
+ uint32_t position_ticks_end;
}stepper_t;
stepper_t stepper_new(pin_t *step_pin, pin_t *dir_pin);
void stepper_reset(stepper_t *stepper);
+// steps discrete, mm/min
void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed);
+// steps discrete, mm/min, mm/min/s (mm/s better but we want more discrete resolution)
+void stepper_block(stepper_t *stepper, int32_t steps, uint32_t entryspeed, uint32_t accel, uint32_t accellength, uint32_t deccellength);
+
void stepper_update(stepper_t *stepper);
/*
--
GitLab