Well, I did more than I thought I could on this, and my robot is nearly standing on it' own. I am using the FspTimer timers and though it's a little awkward to program them, they are working. I'm having a little trouble with one of the 50 year old steppers. If I can't clear it, I may need to switch to something more recent which means making a new body and wheels. I'll do that eventually anyway because these old ones and the huge battery weigh a ton. It will be tough for the servos to raise it when it falls over.
This is here for review. Positive comments, criticisms welcome.
I'll be out of town this weekend. When I get back on Monday, I'll probable make a new robot using standard NEMA 14 motors and work on this code some more.
example.ino
#include "robot_stepper.h"
#include <PID_v1.h>
#include "FspTimer.h"
//#define PID_DEBUG
#define IMU_DEBUG
FspTimer M1_timer;
FspTimer M2_timer;
/************************************************************************************************************************/
#include <MPU6050_tockn.h> // will change to SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic) when parts arrive
/************************************************************************************************************************/
#include <Wire.h>
/************************************************************************************************************************/
MPU6050 robotIMU(Wire); // will change to SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic) when parts arrive
/************************************************************************************************************************/
#define ENABLE_PIN 4 // a common enable pin for both of my motors
#define STEP1_PIN 7 // pulses LOW for 1 usec to move motor 1 one step
#define DIR1_PIN 8 // determines direction, FWD/REV for motor 1
#define STEP2_PIN 12 // pulses LOW for 1 usec to move motor 2 one step
#define DIR2_PIN 5 // determines direction, FWD/REV for motor 2
#define MAX_SPEED 100 // sets the maximum stepper speed in RPM
// PID parameters
double setpoint, input, output; // variables we'll be connecting to
double cons_kp = 1, cons_ki = 0.05, cons_kd = 0.25; // conservative tuning Parameters (when the robot nearly upright)
double agg_kp = 4, agg_ki = 0.2, agg_kd = 1; // aggressive tuning Parameters (when the robot is far from upright);
double acc, gyro, accOld, gyroOld; // current and old data from the imu
unsigned long loopTimer, loopTimerOld; // main loop timer
unsigned long reportTimer, startReportTimer;
#define ACC_STEPS_SEC 10 // the user's acceleration rate
StepperClass M1; // the motor 1 instance
StepperClass M2; // the motor 2 instance
//Specify the links and initial tuning parameters
PID robotPID(&input, &output, &setpoint, cons_kp, cons_ki, cons_kd, DIRECT);
// setup sets the initial state of the motor driver pins and each motor's unique timer
/************************************************************************************************************************/
void setup() {
Serial.begin(115200);
delay(500);
pinMode(ENABLE_PIN, OUTPUT); // this is the motor driver enable pin
digitalWrite(ENABLE_PIN, HIGH); // a HIGH disables the motors while we configure things
M1.initialize(STEP1_PIN, DIR1_PIN); // initialize motor1 and its timer
M2.initialize(STEP2_PIN, DIR2_PIN); // initialize motor2 and its timer
M1.getFrequency(0); // motors run in opposite directions for straight robot movement
M2.getFrequency(0); // motors run in opposite directions for straight robot movement
M1.setAccelStepsSec(10); // sets the number of steps per sec increase or decrease for acceleration
digitalWrite(ENABLE_PIN, LOW); // now we can enable the motors (a LOW enables the motors)
setpoint = 0; // <set to whatever value the imu reports when it is uoright>
robotPID.SetMode(AUTOMATIC);
robotPID.SetOutputLimits(-100, 100); // These limits will be useful for the timer calculations
// start the IMU
Wire.begin();
/************************************************************************************************************************/
robotIMU.begin();
// robotIMU.calcGyroOffsets(true);
/************************************************************************************************************************/
//turn the PID on
robotPID.SetMode(AUTOMATIC);
loopTimerOld = millis();
beginTimerM1(M1.getFrequency(25));
beginTimerM2(M2.getFrequency(25));
delay(2000);
}
// loop
/************************************************************************************************************************/
void loop() {
// loopTimer = millis();
// if (loopTimer - loopTimerOld > 20) {
loopTimerOld = loopTimer;
/************************************************************************************************************************/
robotIMU.update();// will change to SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic) when parts arrive
/************************************************************************************************************************/
double gap = abs(setpoint - input); //distance away from setpoint
if (gap < 10)
{ //we're close to setpoint, use conservative tuning parameters
robotPID.SetTunings(cons_kp, cons_ki, cons_kd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
robotPID.SetTunings(agg_kp, agg_ki, agg_kd);
}
// position the imu sensor on the robot so the X angle changes when the robot is tilted back and forth
/************************************************************************************************************************/
input = robotIMU.getAngleX(); // will change to SparkFun 9DoF IMU Breakout - ICM-20948 (Qwiic) when parts arrive
#ifdef IMU_DEBUG
Serial.print("angle "); Serial.println(input);
#endif
/************************************************************************************************************************/
/* <do any necessary imu signal filtering here>
Examples:
acc = acc * 0.8 + accOld * 0.2;
accOld = acc;
gyro = gyro * 0.8 + gyroOld * 0.2;
gyroOld = gyro;
*/
// run the PID routine
robotPID.Compute(); // output value will be -100 to 100
#ifdef PID_DEBUG
Serial.print("PID Input "); Serial.print(input);
Serial.print(" PID output "); Serial.println(output);
#endif
float m1, m2;
m1 = M1.getFrequency(output);
m2 = M1.getFrequency(output);
Serial.print(m1); Serial.print(" "); Serial.println(m2);
M1_timer.set_frequency(m1);
M2_timer.set_frequency(m2);
reportTimer = millis();
if (reportTimer - startReportTimer > 2000); // send a report to the Serial monitor every 2 second
reportStatus();
}
//}
// reportStatus
/************************************************************************************************************************/
void reportStatus() {
}
// callback method used by M1_timer
void M1_callback(timer_callback_args_t __attribute((unused)) *p_args) {
// digitalWrite(M1.stepPin, LOW);
delayMicroseconds(2);
digitalWrite(M1.stepPin, HIGH);
}
// callback method used by M2_timer
void M2_callback(timer_callback_args_t __attribute((unused)) *p_args) {
digitalWrite(M2.stepPin, LOW);
delayMicroseconds(2);
digitalWrite(M2.stepPin, HIGH);
}
// beginTimerM1
/************************************************************************************************************************/
bool beginTimerM1(float rate) {
uint8_t timer_type = GPT_TIMER;
int8_t tindex = FspTimer::get_available_timer(timer_type);
if (tindex < 0) {
tindex = FspTimer::get_available_timer(timer_type, true);
}
if (tindex < 0) {
return false;
}
FspTimer::force_use_of_pwm_reserved_timer();
if (!M1_timer.begin(TIMER_MODE_PERIODIC, timer_type, tindex, rate, 0.0f, M1_callback)) {
return false;
}
if (!M1_timer.setup_overflow_irq()) {
return false;
}
if (!M1_timer.open()) {
return false;
}
if (!M1_timer.start()) {
return false;
}
return true;
}
// beginTimerM2
/************************************************************************************************************************/
bool beginTimerM2(float rate) {
uint8_t timer_type = GPT_TIMER;
int8_t tindex = FspTimer::get_available_timer(timer_type);
if (tindex < 0) {
tindex = FspTimer::get_available_timer(timer_type, true);
}
if (tindex < 0) {
return false;
}
FspTimer::force_use_of_pwm_reserved_timer();
if (!M2_timer.begin(TIMER_MODE_PERIODIC, timer_type, tindex, rate, 0.0f, M2_callback)) {
return false;
}
if (!M2_timer.setup_overflow_irq()) {
return false;
}
if (!M2_timer.open()) {
return false;
}
if (!M2_timer.start()) {
return false;
}
return true;
}
robot_stepper.cpp
#include "robot_stepper.h"
StepperClass::StepperClass() {};
/* initialize gets the motor ready by initializing its unique timer and
configuring the pins for the stepper motor.
initialize */
/************************************************************************************************************************/
void StepperClass::initialize(int _stepPin, int _dirPin) {
stepPin = _stepPin;
dirPin = _dirPin;
pinMode(stepPin, OUTPUT);
pinMode(dirPin, OUTPUT);
digitalWrite(dirPin, HIGH);
}
/************************************************************************************************************************/
float StepperClass::getFrequency(float _desiredRPM) {
desiredRPM = constrain(_desiredRPM, -maxRPM, maxRPM);
desiredRPM = map(_desiredRPM, -maxRPM, maxRPM, maxRPM, -maxRPM);
if (_desiredRPM >= 0)
digitalWrite(dirPin, HIGH);
else
digitalWrite(dirPin, LOW);
float rps = _desiredRPM / 60;
float frequency = 3200 * rps;
return abs(frequency);
}
// setAccelStepsSec
/************************************************************************************************************************/
void StepperClass::setAccelStepsSec(boolean _accelStepsSec) { // the default is DIRECT, but PERCENTAGE is also supported
accelStepsSec = _accelStepsSec;
}
// setMaxRPM
/************************************************************************************************************************/
void StepperClass::setMaxRPM(int _maxRPM) {
maxRPM = _maxRPM;
}
// setStepsPerRevolution
/************************************************************************************************************************/
void StepperClass::setStepsPerRevolution(int _steps) {
stepsPerRevolution = _steps;
}
// raiseRobot
/************************************************************************************************************************/
void StepperClass::raiseRobot() {
/* If the robot is tilted past controllable angle, operate
the servs(s) to raise it to a controllable state.
*/
}
// disable
/************************************************************************************************************************/
void StepperClass::disableSteppers(int enablePin) {
// stop the motor
getFrequency(0);
// the disable the motors
digitalWrite(enablePin, HIGH);
}
// enable
/************************************************************************************************************************/
void StepperClass::enableSteppers(int enablePin) {
digitalWrite(enablePin, LOW);
}
// OSCReadMsg
/************************************************************************************************************************/
char StepperClass::OSCReadMsg() {
// <tbd>
}
// OSCSendMsg
/************************************************************************************************************************/
void StepperClass::OSCSendMsg() {
// <tbd>
}
robot_stepper.h
#ifndef __ROBOT__
#define __ROBOT__
#include <Arduino.h>
// StepperClass
/************************************************************************************************************************/
class StepperClass {
public:
enum
{
DIRECT_RPM,
PERCENT_MAX_RPM,
MOVE_FWD,
MOVE_REV
};
private:
int dirPin;
int stepsPerRevolution = 200;
int maxRPM;
int desiredRPM = 20;
int currentRPM = 0;
int accelStepsSec = 5;
boolean currentDirection = MOVE_FWD;
boolean mode = DIRECT_RPM;
unsigned long startTime, currentTime;
public:
int stepPin;
StepperClass();
void initialize(int _stepPin, int _dirPin);
void enableSteppers(int _enablePin);
void disableSteppers(int _enablePin);
float getFrequency(float _desiredRPM);
void setStepsPerRevolution(int _steps);
void setAccelStepsSec(boolean _accelStepsSec);
void setInputMode(boolean _mode);
void setMaxRPM(int _maxRPM);
void raiseRobot();
char OSCReadMsg();
void OSCSendMsg();
};
#endif
