I am trying to use a Pololu Simple Motor Controller with a PID controller to balance a rotary pendulum and I am having some issues. At the current moment the motor controller senses direction and moves if the pendulum is barely moved, but if the pendulum moves fast it gets stuck in a rut spinning at some random fixed rpm.
Background on the project, I have 2 rotary encoders that appear to properly be reading the position and velocity of the pendulum and arm and outputting the data in the serial monitor. So I know the data is right. When the pendulum is positive it reads positive and when it is negative it reads negative. Same with velocity. But as soon as the motor controller starts the data in the serial monitor stops and motor gets stuck at whatever velocity it has in the system. If I move the pendulum fast one way it will go fast, if I move it faster it will get stuck just spinning faster.
I'm guessing something is wrong with the portion of the code for the Pololu 18v7 Simple Motor controller... But I got it from the data sheet so I'm not sure where I'm going wrong!
Hopefully can point me in the right direction.
Here's the code pertaining to the motor controller:
This is at the beginning. Transmitting from Mega2560 on pin 18 to motor controller
#include <SoftwareSerial.h>
#define rxPin 19 // pin 3 connects to smcSerial TX (not used in this example)
#define txPin 18 // pin 4 connects to smcSerial RX
SoftwareSerial smcSerial = SoftwareSerial(rxPin, txPin);
this is in the middle
void setup()
{
//motor control code
// initialize software serial object with baud rate of 38.4 kbps
smcSerial.begin(38400);
// the simple motor controller must be running for at least 1 ms
// before we try to send serial data, so we delay here for 5 ms
delay(5);
// motor controller automatic baud detection
// send it the byte 170 in decimal so that it can learn the baud rate
smcSerial.write(0xAA);
// exit safe start command clears the safe-start violation and lets the motor run
exitSafeStart();
this is the motor logic and serial monitor stuff.
//for debugging in serial monitor outputs error's and motor input
Serial.begin (9600);
}
void loop()
{
u = (kp*proportional + kd*derivative + ki);
motor_input = 3200 / 12 * u; // 12V power supply is used to power the motor at max 3200
motor_input = (int)motor_input;
if (motor_input >= INPUT_MAX){
motor_input = INPUT_MAX;
}
else if (motor_input <= -INPUT_MAX){
motor_input = -INPUT_MAX;
}
Serial.print(proportional);
Serial.print(" ");
Serial.print(derivative);
Serial.print(" ");
Serial.print(integral);
Serial.print(" ");
Serial.println(motor_input);
setMotorSpeed(motor_input); // full speed is 3200
}
and finally the motor communication
// required to allow motors to move
// must be called when controller restarts and after any error
void exitSafeStart()
{
smcSerial.write(0x83);
}
// speed should be a number from -3200 to 3200
void setMotorSpeed(int speed)
{
if (speed < 0)
{
smcSerial.write(0x86); // motor reverse command
speed = -speed; // make speed positive
}
else
{
smcSerial.write(0x85); // motor forward command
}
smcSerial.write(speed & 0x1F);
smcSerial.write(speed >> 5);
}
Is there something wrong with any of this? Why am I getting stuck at some arbitrary motor speed?