diff --git a/motor_controller/Stepper.cpp b/motor_controller/Stepper.cpp new file mode 100644 index 0000000..ec6db41 --- /dev/null +++ b/motor_controller/Stepper.cpp @@ -0,0 +1,74 @@ +#include +#include + +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); + } +} diff --git a/motor_controller/Stepper.h b/motor_controller/Stepper.h new file mode 100644 index 0000000..5ac8675 --- /dev/null +++ b/motor_controller/Stepper.h @@ -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 diff --git a/motor_controller/motor_controller.ino b/motor_controller/motor_controller.ino index d1c6974..dde1b86 100644 --- a/motor_controller/motor_controller.ino +++ b/motor_controller/motor_controller.ino @@ -1,162 +1,79 @@ -# define M_PI 3.14159265358979323846 -#include #include +#include 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);