Skip to content

Refactor stepper code into own class #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rui-lin wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Refactor stepper code into own class #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aadaf37
Refactor stepper code into own class
rui-lin May 14, 2020
e63a8b9
Negate target_step_num_ to match step_num_
rui-lin May 14, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 74 additions & 0 deletions motor_controller/Stepper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
#include <math.h>
#include <Stepper.h>

Stepper::Stepper()
: motor_power_(motor_maxpower),
velocity_(1),
step_num_(0)
{
fill_microstep_array();
}

void Stepper::attach(int Af, int Ab, int Bf, int Bb) {
pinMode(Af, OUTPUT);
pinMode(Ab, OUTPUT);
pinMode(Bf, OUTPUT);
pinMode(Bb, OUTPUT);
Af_ = Af;
Ab_ = Ab;
Bf_ = Bf;
Bb_ = Bb;
digitalWrite(Af, HIGH);
}

void Stepper::setPower(float power_normalized) {
motor_power_ = (int)nearbyintf(power_normalized * motor_maxpower);

fill_microstep_array();
}

void Stepper::setTargetAngle(float angle_in_degrees) {
target_step_num_ = (int)nearbyintf(-angle_in_degrees * steps_per_degree);
}

void Stepper::update() {
int dir = step_num_ - target_step_num_;
if (abs(dir) > 100 && velocity_ < 5)
velocity_ += 0.02;
else if((velocity_ > 1) && (abs(dir) < 100))
velocity_ -= 0.02;
if (dir < 0)
step_num_ += 1;
if (dir > 0)
step_num_ += -1;

step();
}

void Stepper::step() {
int A = microstep_table_[(microstep_table_entries + (step_num_ % microstep_table_entries)) % microstep_table_entries][0];
int B = microstep_table_[(microstep_table_entries + (step_num_ % microstep_table_entries)) % microstep_table_entries][1];
if(A > 0){
analogWrite(Af_, A);
analogWrite(Ab_, 0);
}
if(A < 0){
analogWrite(Ab_, -A);
analogWrite(Af_, 0);
}
if(B > 0){
analogWrite(Bf_, B);
analogWrite(Bb_, 0);
}
if(B < 0){
analogWrite(Bb_, -B);
analogWrite(Bf_, 0);
}
}

void Stepper::fill_microstep_array() {
for(int i = 0; i < microstep_table_entries; i++){
microstep_table_[i][0] = motor_power_ * cos((M_PI*i/microsteps)/2);
microstep_table_[i][1] = motor_power_ * sin((M_PI*i/microsteps)/2);
}
}
47 changes: 47 additions & 0 deletions motor_controller/Stepper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
#ifndef Stepper_h
#define Stepper_h

#include "Arduino.h"

# define M_PI 3.14159265358979323846
constexpr int microsteps = 16;
constexpr int microstep_table_entries = 4*microsteps; // microsteps per electrical cycle
constexpr float steps_per_degree = microsteps * 200 / 360.0;
constexpr int motor_maxpower = 255;

class Stepper
{
public:
Stepper();

void attach(int Af, int Ab, int Bf, int Bb);

// Sets target angle in degrees
void setTargetAngle(float angle_in_degrees);

// power is float in range [0, 1]
void setPower(float power_normalized);

// Should be called every step delay
void update();

float getVelocity() { return velocity_; }
float getCurrentAngle() { return -step_num_ / steps_per_degree; }
float getTargetAngle() { return -target_step_num_ / steps_per_degree; }
float getStepNum() { return step_num_; }
float getNormalizedPower() { return 1.0 * motor_power_ / motor_maxpower; }

private:
void step();
void fill_microstep_array();

int target_step_num_;
int step_num_;
int motor_power_;
float velocity_;
int Af_, Ab_, Bf_, Bb_;
int microstep_table_[microstep_table_entries][2];
};


#endif // Stepper_h
141 changes: 29 additions & 112 deletions motor_controller/motor_controller.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,162 +1,79 @@
# define M_PI 3.14159265358979323846
#include <math.h>
#include <Servo.h>
#include <Stepper.h>

Servo myServo;
Stepper pan_stepper;

float pan_velocity = 1;
constexpr int pan_motor_maxpower = 255;
int pan_motor_power = pan_motor_maxpower;
int step_delay = 3000;
float tilt_angle = 10;
int stepnum = 0;
int pan_stepnum_setpoint = 0;
constexpr int microsteps = 16;
float steps_per_degree = microsteps * 200 / 360.0;

char mode = '\0';

void setup() {
// initialize digital pin LED_BUILTIN as an output.
pinMode(LED_BUILTIN, OUTPUT);
pinMode(PB6, OUTPUT);
pinMode(PB7, OUTPUT);
pinMode(PB8, OUTPUT);
pinMode(PA10, OUTPUT);
pan_stepper.attach(PB6, PB7, PB8, PA10);
myServo.attach(PB9);
Serial.begin(115200); // open a serial connection to your computer
fill_microstep_array();
//analogWriteFrequency(2000);
}

constexpr auto Af = PB6;
constexpr auto Ab = PB7;
constexpr auto Bf = PB8;
constexpr auto Bb = PA10;

void pwmWrite(int pin, int value){
analogWrite(pin, (value==HIGH)*pan_motor_power);
}

constexpr int microstep_table_entries = 4*microsteps; //microsteps per electrical cycle
int microstep_table[microstep_table_entries][2];

void fill_microstep_array(){
for(int i = 0; i < microstep_table_entries; i++){
microstep_table[i][0] = pan_motor_power * cos((M_PI*i/microsteps)/2);
microstep_table[i][1] = pan_motor_power * sin((M_PI*i/microsteps)/2);
}
}

void pan_step(int stepnum) {
delayMicroseconds(step_delay/pan_velocity);
// switch ((8 + (stepnum % 8)) % 8) {
// case 0:
// pwmWrite(Bf, HIGH);
// break;
// case 1:
// pwmWrite(Af, LOW);
// break;
// case 2:
// pwmWrite(Ab, HIGH);
// break;
// case 3:
// pwmWrite(Bf, LOW);
// break;
// case 4:
// pwmWrite(Bb, HIGH);
// break;
// case 5:
// pwmWrite(Ab, LOW);
// break;
// case 6:
// pwmWrite(Af, HIGH);
// break;
// case 7:
// pwmWrite(Bb, LOW);
// break;
//
// }
int A = microstep_table[(microstep_table_entries + (stepnum % microstep_table_entries)) % microstep_table_entries][0];
int B = microstep_table[(microstep_table_entries + (stepnum % microstep_table_entries)) % microstep_table_entries][1];
if(A > 0){
analogWrite(Af, A);
analogWrite(Ab, 0);
}
if(A < 0){
analogWrite(Ab, -A);
analogWrite(Af, 0);
}
if(B > 0){
analogWrite(Bf, B);
analogWrite(Bb, 0);
}
if(B < 0){
analogWrite(Bb, -B);
analogWrite(Bf, 0);
}
}


void loop() {
digitalWrite(Af, HIGH);
for (int loopnum = 0;; loopnum++) {
int dir = stepnum + pan_stepnum_setpoint;
if (abs(dir) > 100 && pan_velocity < 5)
pan_velocity+=0.02;
else if((pan_velocity > 1) && (abs(dir) < 100))
pan_velocity-=0.02;
if (dir < 0)
stepnum += 1;
if (dir > 0)
stepnum += -1;
// TODO: update constant force code so it does not rely on a variable sleep.
// This will allow us to run multiple steppers at once.
pan_stepper.update();
delayMicroseconds(step_delay/pan_stepper.getVelocity());

pan_step(stepnum);
while (Serial.available()) {
switch (Serial.read()) {
case 's': //Set
mode = 's';
break;
case 't': //Tilt angle
if (mode == 's')
if (mode == 's') {
tilt_angle = Serial.parseFloat();
}
mode = '\0';
break;
case 'p': //Pan angle
if (mode == 's')
pan_stepnum_setpoint = (int)nearbyintf(Serial.parseFloat() * steps_per_degree);
if (mode == 's') {
pan_stepper.setTargetAngle(Serial.parseFloat());
}
mode = '\0';
break;
case 'w': //motor power
if (mode == 's'){
pan_motor_power = (int)nearbyintf(Serial.parseFloat() * pan_motor_maxpower);
fill_microstep_array();
if (mode == 's') {
pan_stepper.setPower(Serial.parseFloat());
}
mode = '\0';
break;
case 'd': //step Delay
if (mode == 's')
if (mode == 's') {
step_delay = Serial.parseInt();
}
mode = '\0';
break;

}
}

if(loopnum % 16 ==0){
if(loopnum % 16 ==0) {
// The following values are used for communication with coralboard
Serial.print("tilt_angle ");
Serial.println(tilt_angle);
Serial.print("stepnum ");
Serial.println(stepnum);
Serial.print("pan_angle_setpoint ");
Serial.println(pan_stepnum_setpoint / steps_per_degree);
Serial.print("pan_angle ");
Serial.println(-stepnum / steps_per_degree);
Serial.print("pan_motor_power ");
Serial.println(1.0*pan_motor_power / pan_motor_maxpower);
Serial.println(pan_stepper.getCurrentAngle());

// The following values are for debug purposes
Serial.print("step_delay ");
Serial.println(step_delay);
Serial.print("pan_step_num ");
Serial.println(pan_stepper.getStepNum());
Serial.print("pan_target_angle ");
Serial.println(pan_stepper.getTargetAngle());
Serial.print("pan_motor_power ");
Serial.println(pan_stepper.getNormalizedPower());
Serial.print("pan_velocity ");
Serial.println(pan_velocity);
Serial.println(pan_stepper.getVelocity());
}
// if(lo!=-1)
// Serial.print((char)lo);
Expand Down