Skip to content
Snippets Groups Projects
Commit cdda2643 authored by Jake Read's avatar Jake Read
Browse files

stepper executes blocks in buffer

parent ef824cd7
Branches
Tags
No related merge requests found
...@@ -62,7 +62,7 @@ void apa_handle_packet(uint8_t *packet, uint8_t length){ ...@@ -62,7 +62,7 @@ void apa_handle_packet(uint8_t *packet, uint8_t length){
// in steps/s // in steps/s
uint32_t deccellength = (packet[i+17] << 24) | (packet[i+18] << 16) | (packet[i+19] << 8) | packet[i+20]; uint32_t deccellength = (packet[i+17] << 24) | (packet[i+18] << 16) | (packet[i+19] << 8) | packet[i+20];
// do the business // do the business
stepper_block(&stepper, steps, entryspeed, accel, accellength, deccellength); stepper_new_block(&stepper, steps, entryspeed, accel, accellength, deccellength);
i += 13; // ? not 12 ? i += 13; // ? not 12 ?
} }
break; break;
......
...@@ -70,6 +70,6 @@ stepper_t stepper; ...@@ -70,6 +70,6 @@ stepper_t stepper;
// DEBUG // DEBUG
pin_t tickuptx; pin_t tickup0tx;
#endif /* HARDWARE_H_ */ #endif /* HARDWARE_H_ */
\ No newline at end of file
...@@ -245,8 +245,10 @@ int main(void) ...@@ -245,8 +245,10 @@ int main(void)
stepper = stepper_new(&step_pin, &dir_pin); stepper = stepper_new(&step_pin, &dir_pin);
stepper_reset(&stepper); stepper_reset(&stepper);
tickuptx = pin_new(&PORT->Group[0], 13); tickup0tx = pin_new(&PORT->Group[0], 13);
pin_output(&tickuptx); pin_output(&tickup0tx);
//pin_clear(&stlb);
while (1) while (1)
{ {
...@@ -265,7 +267,7 @@ next steps (haha) ...@@ -265,7 +267,7 @@ next steps (haha)
void SysTick_Handler(void){ void SysTick_Handler(void){
// slow ticker // slow ticker
pin_toggle(&stlb); //pin_toggle(&stlb);
//uart_sendchar_buffered(&up0, 120); //uart_sendchar_buffered(&up0, 120);
...@@ -305,6 +307,6 @@ void TC0_Handler(void){ // fires rarely, for counting overflows of time-ticker ...@@ -305,6 +307,6 @@ void TC0_Handler(void){ // fires rarely, for counting overflows of time-ticker
void TC2_Handler(void){ // fires every 8.3us, for step checking void TC2_Handler(void){ // fires every 8.3us, for step checking
TC2->COUNT32.INTFLAG.bit.MC0 = 1; TC2->COUNT32.INTFLAG.bit.MC0 = 1;
TC2->COUNT32.INTFLAG.bit.MC1 = 1; TC2->COUNT32.INTFLAG.bit.MC1 = 1;
pin_toggle(&tickuptx); pin_toggle(&tickup0tx);
stepper_update(&stepper); stepper_update(&stepper);
} }
\ No newline at end of file
...@@ -29,6 +29,8 @@ uint8_t rb_empty(ringbuffer_t *rb){ ...@@ -29,6 +29,8 @@ uint8_t rb_empty(ringbuffer_t *rb){
} }
uint8_t rb_full(ringbuffer_t *rb){ uint8_t rb_full(ringbuffer_t *rb){
// read from tail, update at head
// if head is 'just behind' tail (in ring) we have no extra space: the
return ((rb->head + 1) % rb->size) == rb->tail; return ((rb->head + 1) % rb->size) == rb->tail;
} }
......
...@@ -40,6 +40,7 @@ void stepper_reset(stepper_t *stepper){ ...@@ -40,6 +40,7 @@ void stepper_reset(stepper_t *stepper){
} }
void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed){ void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed){
// TODO: update to ship a block
// set speed period // set speed period
(speed < 1) ? speed = 1 : (0); // avoid 0 division, 1 step / s seems like reasonable lower bound step rate (speed < 1) ? speed = 1 : (0); // avoid 0 division, 1 step / s seems like reasonable lower bound step rate
stepper->speed = speed; stepper->speed = speed;
...@@ -65,52 +66,85 @@ void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed){ ...@@ -65,52 +66,85 @@ void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed){
stepper->last_step = 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){ void stepper_new_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 // does assignments and adds to queue
// TODO: should block the execution of this block while we do this, so that we
// have an interrupt safe ringbuffer
// enforce no div/0 // enforce no div/0
(entryspeed < 1) ? entryspeed = 1 : (0); (entryspeed < 1) ? entryspeed = 1 : (0);
// going to have to catch blocks which cause deceleration to 0 during deceleration phases ! // going to have to catch blocks which cause deceleration to 0 during deceleration phases !
stepper->speed = entryspeed; stepper->block[stepper->blockhead].entry_speed = entryspeed;
// do starting speed period // do starting speed period
stepper->speed_period = stepper->one_minute / stepper->speed; stepper->block[stepper->blockhead].accel_period = stepper->one_minute / accel;
stepper->accel_period = stepper->one_minute / accel;
// set dir // set dir
if(steps < 0){ if(steps < 0){
pin_clear(stepper->dir_pin); stepper->block[stepper->blockhead].dir = 0;
} else { } else {
pin_set(stepper->dir_pin); stepper->block[stepper->blockhead].dir = 1;
} }
// do lengths // do lengths
stepper->position_ticks_end = abs(steps); stepper->block[stepper->blockhead].position_end = abs(steps);
stepper->position_accel_to = accellength; stepper->block[stepper->blockhead].position_accel_to = accellength;
stepper->position_deccel_from = deccellength; stepper->block[stepper->blockhead].position_deccel_from = deccellength;
stepper->position_ticks = 0;
// ready set
stepper->block[stepper->blockhead].is_new = 1;
// increment block head ptr: should catch full queue HERE but not bothering
stepper->blockhead = (stepper->blockhead + 1) % stepper->blocksize;
// reset times // reset times
/*
TICKER_SYNC; TICKER_SYNC;
unsigned long now = TICKER; unsigned long now = TICKER;
stepper->last_accel = now; stepper->last_accel = now;
stepper->last_step = now; stepper->last_step = now;
*/
} }
void stepper_update(stepper_t *stepper){ void stepper_update(stepper_t *stepper){
if(stepper->position_ticks_end > stepper->position_ticks){ // still have somewhere to go if(stepper->blockhead == stepper->blocktail){
// pin_clear(&stlb);
// bail, no steps to make, ringbuffer is empty
} else if(stepper->block[stepper->blocktail].position_end > stepper->position_ticks){
pin_set(&stlb);
// we have somewhere to go
TICKER_SYNC; TICKER_SYNC;
unsigned long now = TICKER; unsigned long now = TICKER;
if(stepper->position_ticks < stepper->position_accel_to){ if(stepper->block[stepper->blocktail].is_new){
// if we're just starting this block, set the speed
stepper->speed = stepper->block[stepper->blocktail].entry_speed;
stepper->speed_period = stepper->one_minute / stepper->speed;
// and set the dir
if(stepper->block[stepper->blocktail].dir < 0){
pin_clear(stepper->dir_pin);
} else {
pin_set(stepper->dir_pin);
}
// and clear the distance
stepper->position_ticks = 0;
// and then clear this flag
stepper->block[stepper->blocktail].is_new = 0;
}
if(stepper->position_ticks < stepper->block[stepper->blocktail].position_accel_to){
// we're accelerating! // we're accelerating!
if(now - stepper->last_accel > stepper->accel_period){ if(now - stepper->last_accel > stepper->block[stepper->blocktail].accel_period){
stepper->speed += 1; stepper->speed += 1;
stepper->speed_period = stepper->one_minute / stepper->speed; stepper->speed_period = stepper->one_minute / stepper->speed;
stepper->last_accel = now; stepper->last_accel = now;
} }
} else if(stepper->position_ticks > stepper->position_deccel_from){ } else if(stepper->position_ticks > stepper->block[stepper->blocktail].position_deccel_from){
if(now - stepper->last_accel > stepper->accel_period){ if(now - stepper->last_accel > stepper->block[stepper->blocktail].accel_period){
stepper->speed -= 1; stepper->speed -= 1;
(stepper->speed < 1) ? stepper->speed = 1 : (0); // assert no 0's (stepper->speed < 1) ? stepper->speed = 1 : (0); // assert no 0's
stepper->speed_period = stepper->one_minute / stepper->speed; stepper->speed_period = stepper->one_minute / stepper->speed;
...@@ -124,10 +158,19 @@ void stepper_update(stepper_t *stepper){ ...@@ -124,10 +158,19 @@ void stepper_update(stepper_t *stepper){
if(now - stepper->last_step >= stepper->speed_period){ if(now - stepper->last_step >= stepper->speed_period){
pin_toggle(stepper->step_pin); pin_toggle(stepper->step_pin);
stepper->position_ticks ++; stepper->position_ticks ++;
stepper->last_step = now; //stepper->last_step + stepper->speed_period; // last speed_period if accelerating 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, // 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 // we tend do equal amounts undershooting speed on all motors
} // end step cycle } // end step cycle
}// end step if not there cycle } else {
pin_clear(&stlb);
// we've just finished a block, next time we'll be running with that, or call an empty queue
stepper->blocktail = (stepper->blocktail + 1) % stepper->blocksize;
stepper->position_ticks = 0;
// call end-block - will want to send the ack now
}
} }
\ No newline at end of file
...@@ -12,7 +12,7 @@ ...@@ -12,7 +12,7 @@
#include "sam.h" #include "sam.h"
#include "pin.h" #include "pin.h"
#define BLOCKS_QUEUE_SIZE 64 #define BLOCKS_QUEUE_SIZE 16
// one movement // one movement
typedef struct { typedef struct {
...@@ -20,11 +20,14 @@ typedef struct { ...@@ -20,11 +20,14 @@ typedef struct {
uint8_t return_address[8]; // C quesion: how to do this properly with malloc() ? malloc() on embedded sys? uint8_t return_address[8]; // C quesion: how to do this properly with malloc() ? malloc() on embedded sys?
uint8_t return_address_length; uint8_t return_address_length;
// tracking
uint8_t is_new;
// for what you do // for what you do
uint8_t dir; // 0 or 1 uint8_t dir; // 0 or 1
uint32_t length; // in steps uint32_t position_end; // in steps
uint32_t entry_speed; uint32_t entry_speed;
uint32_t accel; uint32_t accel_period;
uint32_t position_accel_to; uint32_t position_accel_to;
uint32_t position_deccel_from; uint32_t position_deccel_from;
}block_t; }block_t;
...@@ -67,7 +70,7 @@ void stepper_reset(stepper_t *stepper); ...@@ -67,7 +70,7 @@ void stepper_reset(stepper_t *stepper);
void stepper_steps(stepper_t *stepper, int32_t steps, uint32_t speed); 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) // 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_new_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); void stepper_update(stepper_t *stepper);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment