eddy/scorpio
1
0
Fork 0
mirror of https://github.com/eddyem/scorpio.git synced 2025-12-06 10:45:15 +03:00
Code Issues Packages Projects Releases Wiki Activity
scorpio/STM8/2stepper4reductor/motors.c

437 lines
15 KiB
C
Raw Blame History

/*
* geany_encoding=koi8-r
* motors.c
*
* Copyright 2018 Edward V. Emelianov <eddy@sao.ru, edward.emelianoff@gmail.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
* MA 02110-1301, USA.
*
*/
#include "motors.h"
#include "uart.h"
#include "hardware.h"
//U8 irq_flag = 0; // |1 - 1st motor, |2 - second
// motor states
typedef enum{
MOTOR_RELAX, // do nothing
MOTOR_INFMOVE, // infinite moving
MOTOR_STOP, // stop
MOTOR_ZEROSTOP, // stop and set current position to zero
MOTOR_MOVENSTEPS, // move for N steps
MOTOR_OFFSWITCH, // try to pull off the positive switch (for turrets and to return from switch back)
} motor_state;
// direction of rotation
typedef enum{
DIR_CW, // clockwise
DIR_CCW, // counter-clockwise
DIR_STOP // stopped
} motor_direction;
static U16 Stepper_speed[2] = {DEFAULT_USTEP_PERIOD, DEFAULT_USTEP_PERIOD}; // length of one MICROstep in us
static volatile motor_direction Dir[2] = {DIR_STOP, DIR_STOP}; // direction of moving
static volatile motor_state state[2] = {MOTOR_RELAX, MOTOR_RELAX};
static long Steps_left[2] = {0,0}; // steppers left (when moving for given steps amount)
static long Current_pos[2] = {0,0}; // current position
static long Steps_left_at_esw[2] = {0,0}; // amount of `Steps_left` when MOTOR_OFFSWITCH activated
static U16 Acceleration[2] = {0, 0}; // current acceleration (to reach target speed by ACCEL_STEPS microsteps)
// microsteps profile
// microsteps: DCBA = 1000, 1010, 0010, 0110, 0100, 0101, 0001, 1001 - half-step
// 1010, 0110, 0101, 1001 - full step
static const U8 usteps[8] = {0b1000, 0b1010, 0b0010, 0b0110, 0b0100, 0b0101, 0b0001, 0b1001};
static U8 Ustep[2] = {0, 0}; // microstep counter
// init timers & GPIO for motors
void motors_init(){
// Configure timer 2 to generate signals for CLK of motor 0
TIM2_PSCR = 4; // 1MHz
TIM2_ARRH = DEFAULT_USTEP_PERIOD >> 8; // set speed
TIM2_ARRL = DEFAULT_USTEP_PERIOD & 0xff;
TIM2_IER = TIM_IER_UIE; // update interrupt enable
TIM2_CR1 |= TIM_CR1_APRE | TIM_CR1_URS; // auto reload + interrupt on overflow
// timer 3 for motor 1
TIM3_PSCR = 4;
TIM3_ARRH = DEFAULT_USTEP_PERIOD >> 8;
TIM3_ARRL = DEFAULT_USTEP_PERIOD & 0xff;
TIM3_IER = TIM_IER_UIE;
TIM3_CR1 |= TIM_CR1_APRE | TIM_CR1_URS;
}
void show_motors_help(){
// "start end"
uart_write("\tE - get end-switches\n");
uart_write("\tL - move CCW\n");
uart_write("\tM - get motor state\n");
uart_write("\tN - go for N st./get rest\n");
uart_write("\tO - pull off the switch\n");
uart_write("\tP - get current position\n");
uart_write("\tR - move CW\n");
uart_write("\tS - get/set speed\n");
uart_write("\tX - stop motor\n");
uart_write("\tZ - stop and zero position\n");
}
/**
* Check endswitches
* @return 0 if none pressed, 1 if "-", 2 if "+", 3 if both!
*/
static U8 check_endsw(U8 motor){
U8 ret = 0;
switch(motor){
case 0:
if(CHK_M0E1()) ++ret;
if(CHK_M0E2()) ret += 2;
break;
case 1:
if(CHK_M1E1()) ++ret;
if(CHK_M1E2()) ret += 2;
break;
default:
return 0;
}
return ret;
}
static void stop_motor(U8 motorNum){
// stop timers & turn off power
switch(motorNum){
case 0:
PORT(STP0_PORT, ODR) &= ~STP_PINS;
TIM2_CR1 &= ~TIM_CR1_CEN;
break;
case 1:
PORT(STP1_PORT, ODR) &= ~STP_PINS;
TIM3_CR1 &= ~TIM_CR1_CEN;
break;
default: return;
}
Steps_left[motorNum] = 0;
Dir[motorNum] = DIR_STOP;
if(state[motorNum] == MOTOR_ZEROSTOP) Current_pos[motorNum] = 0;
state[motorNum] = MOTOR_RELAX;
}
static void strtobuf(const char *str, char **buff){
while(*str) *((*buff)++) = *str++;
}
static void get_motor_state(U8 nmotor, char **buff){
char sig = '+';
if(nmotor > 1) return;
if(Dir[nmotor] == DIR_CCW) sig = '-';
switch(state[nmotor]){
case MOTOR_RELAX:
strtobuf("RELAX", buff);
break;
case MOTOR_INFMOVE:
strtobuf("INFMV", buff); // INVMV+ or INFMV-
*((*buff)++) = sig;
break;
break;
case MOTOR_STOP:
strtobuf("STOP", buff);
break;
case MOTOR_MOVENSTEPS:
strtobuf("MVSTP", buff); // MVSTP+ or MVSTP-
*((*buff)++) = sig;
break;
case MOTOR_OFFSWITCH:
strtobuf("OFFSW", buff); // OFFSW+ or OFFSW-
*((*buff)++) = sig;
break;
default:
strtobuf("UNDEF", buff);
}
}
// turn on motor's timer starting from the lowest speed
static void turnontimer(U8 motorNum){
U8 tmp;
switch(motorNum){
case 0:
// turn on power
tmp = PORT(STP0_PORT, ODR) & ~STP_PINS;
PORT(STP0_PORT, ODR) = tmp | usteps[Ustep[0]];
// start from the slowest speed
TIM2_ARRH = MAX_USTEP_PERIOD >> 8;
TIM2_ARRL = MAX_USTEP_PERIOD & 0xff;
// run timer
TIM2_CR1 |= TIM_CR1_CEN;
break;
case 1:
tmp = PORT(STP1_PORT, ODR) & ~STP_PINS;
PORT(STP1_PORT, ODR) = tmp | usteps[Ustep[1]];
TIM3_ARRH = MAX_USTEP_PERIOD >> 8;
TIM3_ARRL = MAX_USTEP_PERIOD & 0xff;
TIM3_CR1 |= TIM_CR1_CEN;
break;
default: return;
}
Acceleration[motorNum] = 1 + (MAX_USTEP_PERIOD - Stepper_speed[motorNum]) / ACCEL_USTEPS;
}
/**
* try to move motor motorNum for nsteps
* @return 1 if all OK; 0 if still moving or on end-switch
*/
static int moveNsteps(U8 motorNum, long nsteps){
U8 sw;
if(Dir[motorNum] != DIR_STOP) return 0;
sw = check_endsw(motorNum);
if(nsteps < 0){
if(sw) return 0; // on zero end-switch: no moving backward, on positive - no moving at all!
Dir[motorNum] = DIR_CCW;
nsteps = -nsteps;
state[motorNum] = MOTOR_MOVENSTEPS;
}else{
if(sw & 2) return 0; // for positive direction no moving to any side when on end-switch 2!
else if(sw & 1) state[motorNum] = MOTOR_OFFSWITCH;
else state[motorNum] = MOTOR_MOVENSTEPS;
Dir[motorNum] = DIR_CW;
}
Steps_left[motorNum] = nsteps;
// turn On timer
turnontimer(motorNum);
return 1;
}
/**
* try to move motor motorNum off the end-switch for nsteps
* @return 1 if all OK; 0 if still moving, motor is @ zero endswitch
*/
static int pullofftheswitch(U8 motorNum, long nsteps){
U8 sw;
if(Dir[motorNum] != DIR_STOP) return 0;
sw = check_endsw(motorNum);
if(sw == 0) return moveNsteps(motorNum, nsteps);
if(nsteps < 0){
if(sw & 1) return 0; // on zero end-switch: no moving backward
Dir[motorNum] = DIR_CCW;
nsteps = -nsteps;
}else{
Dir[motorNum] = DIR_CW;
}
Steps_left[motorNum] = nsteps;
Steps_left_at_esw[motorNum] = nsteps;
state[motorNum] = MOTOR_OFFSWITCH;
turnontimer(motorNum);
return 1;
}
void motor_command(const char *cmd, char **buff){
U8 motorNum = *cmd++ - '0';
U16 spd;
long l;
char c;
if(motorNum > 1) goto someerr;
*((*buff)++) = '0' + motorNum;
*((*buff)++) = ' ';
c = *cmd++;
*((*buff)++) = c;
switch(c){
case 'E': // check endswitches state
*((*buff)++) = ' ';
*((*buff)++) = '0' + check_endsw(motorNum);
break;
case 'L': // infinite move left
if(check_endsw(motorNum) & 1){
strtobuf(" E 1", buff);
}else{
state[motorNum] = MOTOR_INFMOVE;
Dir[motorNum] = DIR_CCW;
turnontimer(motorNum);
}
break;
case 'M': // get motor state
*((*buff)++) = ' ';
get_motor_state(motorNum, buff);
break;
case 'N': // go for N steps or get steps left
*((*buff)++) = ' ';
if(!readLong(cmd, &l)){ // get
long2buf(Steps_left[motorNum], buff);
}else{
if(!moveNsteps(motorNum, l)) goto someerr;
else long2buf(l, buff);
}
break;
case 'O': // pull off the switch (if no steps given, go for PULLOFFTHESW_STEPS)
*((*buff)++) = ' ';
if(!readLong(cmd, &l)){ // get
l = PULLOFFTHESW_STEPS;
}
if(!pullofftheswitch(motorNum, l)) goto someerr;
else long2buf(l, buff);
break;
case 'P': // get current position
*((*buff)++) = ' ';
long2buf(Current_pos[motorNum], buff);
break;
case 'R': // infinite move right
if(check_endsw(motorNum) & 2){
strtobuf(" E 2", buff);
}else{
state[motorNum] = MOTOR_INFMOVE;
Dir[motorNum] = DIR_CW;
turnontimer(motorNum);
}
break;
case 'S': // change speed
*((*buff)++) = ' ';
if(!readLong(cmd, &l) || l < MIN_USTEP_PERIOD || l > MAX_USTEP_PERIOD){ // get speed
if(motorNum == 0) spd = TIM2_ARRH << 8 | TIM2_ARRL;
else spd = TIM3_ARRH << 8 | TIM3_ARRL;
long2buf(spd, buff);
}else{
long2buf(l, buff);
spd = (U16)l;
Stepper_speed[motorNum] = spd;
Acceleration[motorNum] = 0;
if(motorNum == 0){
TIM2_ARRH = spd >> 8;
TIM2_ARRL = spd & 0xff;
}else{
TIM3_ARRH = spd >> 8;
TIM3_ARRL = spd & 0xff;
}
}
break;
case 'X': // stop
state[motorNum] = MOTOR_STOP;
break;
case 'Z':
state[motorNum] = MOTOR_ZEROSTOP;
break;
default: // return err
goto someerr;
}
return;
someerr:
*((*buff)++) = 'e';
*((*buff)++) = 'r';
*((*buff)++) = 'r';
}
/**
* this function calls from timer interrupt (TIM2 or TIM3 - motors 0/1)
*/
void stepper_interrupt(U8 motor_num){
U8 tmp;
U16 spd;
U8 sw = check_endsw(motor_num), st = state[motor_num];
U8 ccw = (Dir[motor_num] == DIR_CCW) ? 1 : 0;
//irq_flag ^= 1 << motor_num;
switch(motor_num){
case 0:
if(Acceleration[0]){
spd = (TIM2_ARRH << 8 | TIM2_ARRL) - Acceleration[0];
if(spd < Stepper_speed[0]){
spd = Stepper_speed[0];
Acceleration[0] = 0;
}
//printUint((U8*)&spd, 2);
//uart_write(" - speed0\n");
TIM2_ARRH = spd >> 8;
TIM2_ARRL = spd & 0xff;
}
tmp = PORT(STP0_PORT, ODR) & ~STP_PINS;
PORT(STP0_PORT, ODR) = tmp | usteps[Ustep[0]];
break;
case 1:
if(Acceleration[1]){
spd = (TIM3_ARRH << 8 | TIM3_ARRL) - Acceleration[1];
if(spd < Stepper_speed[1]){
spd = Stepper_speed[1];
Acceleration[1] = 0;
}
//printUint((U8*)&spd, 2);
//uart_write(" - speed1\n");
TIM3_ARRH = spd >> 8;
TIM3_ARRL = spd & 0xff;
}
tmp = PORT(STP1_PORT, ODR) & ~STP_PINS;
PORT(STP1_PORT, ODR) = tmp | usteps[Ustep[1]];
break;
default: return;
}
if(Ustep[motor_num] % 2 == 0){ // full amount of half-steps - increment step counters & check for stop
if(0 == --Steps_left[motor_num]) st = MOTOR_STOP;
if(ccw) --Current_pos[motor_num];
else ++Current_pos[motor_num];
if(st == MOTOR_STOP || st == MOTOR_ZEROSTOP){
stop_motor(motor_num);
return;
}
}
if(ccw){ // counter-clockwise
if(Ustep[motor_num] == 0) Ustep[motor_num] = 7;
else --Ustep[motor_num];
}else{ // clockwise
if(++Ustep[motor_num] > 7) Ustep[motor_num] = 0;
}
switch(st){
case MOTOR_OFFSWITCH: // don't care about endswitch for first PULLOFFTHESW_STEPS steps
if(Steps_left_at_esw[motor_num] - Steps_left[motor_num] >= PULLOFFTHESW_STEPS)
state[motor_num] = MOTOR_MOVENSTEPS;
break;
case MOTOR_MOVENSTEPS:
case MOTOR_INFMOVE: // set curpos to zero only in this state (after reaching ESW1)
if(sw){
if(ccw){
if(st == MOTOR_INFMOVE){
if(sw & 1) state[motor_num] = MOTOR_ZEROSTOP; // esw1 - stop @ zero when inf. left move
}else state[motor_num] = MOTOR_STOP; // just stop at any esw in steps move
}else{ // +switch when move CW
if(sw & 2) state[motor_num] = MOTOR_STOP; // stop in CW only on esw2 !!!
}
}
break;
default: break;
}
}
/**
* Main state-machine process
*
void process_stepper(U8 motor_num){
U8 sw = check_endsw(motor_num);
U8 ccw = (Dir[motor_num] == DIR_CCW) ? 1 : 0;
switch(state[motor_num]){
case MOTOR_OFFSWITCH: // don't care about endswitch for first PULLOFFTHESW_STEPS steps
if(Steps_left[motor_num] < 2) state[motor_num] = MOTOR_STOP;
else if(Steps_left_at_esw[motor_num] - Steps_left[motor_num] >= PULLOFFTHESW_STEPS)
state[motor_num] = MOTOR_MOVENSTEPS;
break;
case MOTOR_MOVENSTEPS:
if(Steps_left[motor_num] < 2) state[motor_num] = MOTOR_STOP; // stop @given position
case MOTOR_INFMOVE: // set curpos to zero only in this state (after reaching ESW1)
if(sw){
if(ccw){
if(state[motor_num] == MOTOR_INFMOVE){
if(sw & 1) state[motor_num] = MOTOR_ZEROSTOP; // esw1 - stop @ zero when inf. left move
}else state[motor_num] = MOTOR_STOP; // just stop at any esw in steps move
}else{ // +switch when move CW
if(sw & 2) state[motor_num] = MOTOR_STOP; // stop in CW only on esw2 !!!
}
}
break;
default: return; // MOTOR_RELAX
}
}*/
Reference in New Issue View Git Blame Copy Permalink