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

absolute barebones stepper wraps, ready to do network things

parent 4f871e62
Branches
No related tags found
No related merge requests found
......@@ -26,6 +26,6 @@ A-la [Mechaduino](http://tropical-labs.com/index.php/mechaduino) and many other
# Currently
Working through firmware setup (and requisite firmware tools / libs for ATSAMD51, will be used on BLDC, etc). Nearly there!
Ready for MKXMods network integration.
![fabbed](https://gitlab.cba.mit.edu/jakeread/mkstepper/raw/master/images/routed.png)
\ No newline at end of file
......@@ -525,3 +525,4 @@ void SysTick_Handler(void){
}
```
OK, this is done now - both timers, and a step to position @ speed command. Next is networks for this board.
\ No newline at end of file
No preview for this file type
......@@ -44,6 +44,7 @@ C_SRCS += \
../pin.c \
../ringbuffer.c \
../spiport.c \
../stepper.c \
../tmc26.c \
../uartport.c
......@@ -61,6 +62,7 @@ main.o \
pin.o \
ringbuffer.o \
spiport.o \
stepper.o \
tmc26.o \
uartport.o
......@@ -71,6 +73,7 @@ main.o \
pin.o \
ringbuffer.o \
spiport.o \
stepper.o \
tmc26.o \
uartport.o
......@@ -81,6 +84,7 @@ main.d \
pin.d \
ringbuffer.d \
spiport.d \
stepper.d \
tmc26.d \
uartport.d
......@@ -91,6 +95,7 @@ main.d \
pin.d \
ringbuffer.d \
spiport.d \
stepper.d \
tmc26.d \
uartport.d
......@@ -127,6 +132,8 @@ LINKER_SCRIPT_DEP+= \
Device_Startup/%.o: ../Device_Startup/%.c
@echo Building file: $<
@echo Invoking: ARM/GNU C Compiler : 6.3.1
......
......@@ -14,6 +14,8 @@ ringbuffer.c
spiport.c
stepper.c
tmc26.c
uartport.c
......
......@@ -14,6 +14,7 @@
#include "spiport.h"
#include "ringbuffer.h"
#include "tmc26.h"
#include "stepper.h"
pin_t stlr;
pin_t stlb;
......@@ -37,4 +38,14 @@ tmc26_t tmc;
uint32_t overflows;
uint32_t blindcounter;
#define STEPTICKER_SYNC (TC2->COUNT32.CTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC)
#define STEPTICKER (TC2->COUNT32.COUNT.reg)
#define TICKER_SYNC (TC0->COUNT32.CTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC)
#define TICKER (TC0->COUNT32.COUNT.reg)
stepper_t stepper;
#endif /* HARDWARE_H_ */
\ No newline at end of file
......@@ -106,9 +106,6 @@ void ticker_init(void){
overflows = 0;
}
#define TICKER_SYNC (TC0->COUNT32.CTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC)
#define TICKER (TC0->COUNT32.COUNT.reg)
void stepticker_init(void){
// Timers: in 32 bit mode we pair two - obscure datasheet reading later, they pair in a predefined way: 0 with 1...
// a word of warning: with the same code, a 16-bit timer was not working. I am mystified.
......@@ -123,8 +120,8 @@ void stepticker_init(void){
GCLK->PCHCTRL[TC2_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(11);
GCLK->PCHCTRL[TC3_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(11);
// turn on in mode, presync
TC2->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV64 | TC_CTRLA_CAPTEN0;// | TC_CTRLA_CAPTEN1;
TC3->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV64 | TC_CTRLA_CAPTEN0;// | TC_CTRLA_CAPTEN1;
TC2->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV16 | TC_CTRLA_CAPTEN0;// | TC_CTRLA_CAPTEN1;
TC3->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV16 | TC_CTRLA_CAPTEN0;// | TC_CTRLA_CAPTEN1;
// do frequency match
TC2->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ;
TC3->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ;
......@@ -133,7 +130,7 @@ void stepticker_init(void){
TC2->COUNT32.INTENSET.bit.MC1 = 1; // don't know why, but had to turn this on to get the interrupts
// set period
while(TC2->COUNT32.SYNCBUSY.bit.CC0);
TC2->COUNT32.CC[0].reg = 11;
TC2->COUNT32.CC[0].reg = 10;
// enable, sync for enable write
TC2->COUNT32.CTRLA.bit.ENABLE = 1;
while(TC2->COUNT32.SYNCBUSY.bit.ENABLE);
......@@ -141,9 +138,6 @@ void stepticker_init(void){
while(TC3->COUNT32.SYNCBUSY.bit.ENABLE);
}
#define STEPTICKER_SYNC (TC2->COUNT32.CTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC)
#define STEPTICKER (TC2->COUNT32.COUNT.reg)
int main(void)
{
/* Initialize the SAM system */
......@@ -222,17 +216,26 @@ int main(void)
pin_output(&step_pin);
pin_output(&dir_pin);
stepper = stepper_new(&step_pin, &dir_pin, 360.0, 64);
stepper_goto(&stepper, 1800, 180); // should do 5 turns in 10 seconds
while (1)
{
// still doin it
//tmc26_update(&tmc);
// delay
for(int i=0; i < 126; i++){
pin_toggle(&stlr);
//pin_toggle(&en_pin);
}
// step
pin_toggle(&step_pin);
/*
next steps (haha)
- poll uart receive line, look for network packets
- on finding position / time period packets, parse, put into buffer of step moves
- roll through buffer of step moves, do them
further reading:
- quadratic interpolation for position @ time w/ velocity @ endpoint step commands
minors:
- inverting direction
- get up to 120MHz for very happy stepping
- bring baud rate to 1M, at least! what the heck, FTDI!
*/
}
}
......@@ -246,17 +249,15 @@ void SysTick_Handler(void){
uart_sendchar_buffered(&up1, rb_get(up1.rbrx));
}
*/
STEPTICKER_SYNC;
uint32_t thecount = STEPTICKER;
uint8_t onemicro = thecount;
uint8_t twomicro = thecount >> 8;
uint8_t threemicro = thecount >> 16;
uint8_t fourmicro = thecount >> 24;
uart_sendchar_buffered(&up1, onemicro);
uart_sendchar_buffered(&up1, twomicro);
uart_sendchar_buffered(&up1, threemicro);
uart_sendchar_buffered(&up1, fourmicro);
uart_sendchar_buffered(&up1, overflows);
int32_t watch = stepper.position_ticks;
uint8_t w1 = watch >> 24;
uint8_t w2 = watch >> 16;
uint8_t w3 = watch >> 8;
uint8_t w4 = watch;
uart_sendchar_buffered(&up1, w1);
uart_sendchar_buffered(&up1, w2);
uart_sendchar_buffered(&up1, w3);
uart_sendchar_buffered(&up1, w4);
}
void SERCOM4_0_Handler(void){
......@@ -283,6 +284,5 @@ void TC0_Handler(void){ // fires rarely, for counting overflows of time-ticker
void TC2_Handler(void){ // fires every 16us, for step checking
TC2->COUNT32.INTFLAG.bit.MC0 = 1;
TC2->COUNT32.INTFLAG.bit.MC1 = 1;
overflows ++;
pin_toggle(&en_pin);
stepper_update(&stepper);
}
\ No newline at end of file
......@@ -190,6 +190,12 @@
<Compile Include="spiport.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="stepper.c">
<SubType>compile</SubType>
</Compile>
<Compile Include="stepper.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="tmc26.c">
<SubType>compile</SubType>
</Compile>
......
/*
* stepper.c
*
* Created: 2/17/2018 5:39:34 PM
* Author: Jake
*/
#include "stepper.h"
#include "hardware.h"
stepper_t stepper_new(pin_t *step_pin, pin_t *dir_pin, float position_per_200_steps, uint32_t microsteps){
stepper_t stepper;
stepper.step_pin = step_pin;
stepper.dir_pin = dir_pin;
stepper.position_per_200_steps = position_per_200_steps;
stepper.microsteps = microsteps;
stepper.speed_period = 0;
stepper.position_ticks = 0;
stepper.position = 0;
stepper.speed = 0;
stepper.position_ticks_target = 0;
return stepper;
}
void stepper_reset(stepper_t *stepper){
stepper->speed_period = 0;
stepper->position_ticks = 0;
stepper->position = 0;
stepper->speed = 0;
stepper->position_ticks_target = 0;
}
// position and speed by position_per_100_steps
// speed is position units / s
void stepper_goto(stepper_t *stepper, float position, float speed){
// in steps / second
uint32_t discrete_speed = (speed / (stepper->position_per_200_steps / 200)) * stepper->microsteps;
// in 3rds of microseconds (a result of timer settings)
stepper->speed_period = 3000000 / discrete_speed;
// in steps pulses
stepper->position_ticks_target = (position / (stepper->position_per_200_steps / 200)) * stepper->microsteps;;
TICKER_SYNC;
stepper->last_time = TICKER;
}
void stepper_update(stepper_t *stepper){
if(stepper->position_ticks_target != stepper->position_ticks){ // still have somewhere to go
TICKER_SYNC;
uint32_t now = TICKER;
if(now - stepper->last_time > stepper->speed_period){
int32_t gap = stepper->position_ticks_target - stepper->position_ticks;
if(gap < 0){
pin_set(stepper->dir_pin); // step forwards
pin_toggle(stepper->step_pin);
stepper->position_ticks --;
} else if (gap > 0) {
pin_clear(stepper->dir_pin);
pin_toggle(stepper->step_pin);
stepper->position_ticks ++;
} else {
// nah
}
stepper->last_time = now;
} // end step cycle
}
}
\ No newline at end of file
/*
* stepper.h
*
* Created: 2/17/2018 5:39:45 PM
* Author: Jake
*/
#ifndef STEPPER_H_
#define STEPPER_H_
#include "sam.h"
#include "pin.h"
typedef struct {
pin_t *step_pin;
pin_t *dir_pin;
// currently
uint32_t speed_period;
int32_t position_ticks;
// updating
uint32_t last_time;
// float conversions
float position_per_200_steps; // units (mm or deg, probably) moved per full rotation
uint32_t microsteps;
float position;
float speed;
// targets
int32_t position_ticks_target;
}stepper_t;
stepper_t stepper_new(pin_t *step_pin, pin_t *dir_pin, float position_per_200_steps, uint32_t microsteps);
void stepper_reset(stepper_t *stepper);
void stepper_goto(stepper_t *stepper, float position, float speed);
void stepper_update(stepper_t *stepper);
void stepper_current_position(stepper_t *steppper, float *position);
void stepper_current_speed(stepper_t *stepper, float *speed);
void stepper_currently(stepper_t *stepper, float *position, float *speed);
#endif /* STEPPER_H_ */
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment