MAAhelp:
Hi all,Started a project around 2 months ago to build an autonomous robot. The wheels and motors I'm using are as follows: http://www.technobotsonline.com/banebot-wheel-124x20mm-1-2-hex-shore-50.html, Pololu - 30:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap).
Although I am able to balance the robot using a PID algorithm, I am finding it tough to control the position of the robot (via the encoders). I have a tilt sensor sending me the angle the robot is at and setting the motorspeed accordingly. To control the position of the robot I have been varying the reference angle to try and move the robot towards a reference point(eg: x = 0) to keep the robot in the same position but I have not had great results.
State space control would be best to control multiple inputs/outputs but I am not sure how this can be done on the Arduino Uno, any ideas?
If state space isn't used how is the best way to approach the problem I am encountering with the position?The code I am using is as follows:
#include "DualVNH5019MotorShield.h"
#include "math.h"
enum PinAssignments {
encoderPinA = 2, // right
encoderPinB = 3, // left
clearButton = 12 // another two pins
};
DualVNH5019MotorShield md;
int sensorPin = A2;
float setpointang = 501.0;
int oldangle, oldangle_1, oldangle_2, angle, newangle, current_1, current_2, errorpos, a, b, c = 0;
double sumangle, output, oldpos, oldpos_1, oldpos_2, derivpos, integpos = 0.0;
int Kp = 20;
int ZoneA = 2000;
int ZoneB = 1000;
float motorspeed, deriv, error, errorposctrl;
double Kd = 0.3;
float freq = 66.67;
float T = ((1/freq)*1000)-0.5;
float Kp_1 = 1.00 ;
float posScaleA = 500.0;
float posScaleB = 1000.0;
float posScaleC = 2000.0;
volatile unsigned int encoderPos = 0; // a counter for the dial
unsigned int lastReportedPos = 1; // change management
static boolean rotating=false; // debounce management
// interrupt service routine vars
boolean A_set = false;
boolean B_set = false;
void setup()
{
Serial.begin(115200);
md.init();
TCCR1B = TCCR1B & 0b11111000 | 0x01;
pinMode(encoderPinA, INPUT);
pinMode(encoderPinB, INPUT);
pinMode(clearButton, INPUT);
// turn on pullup resistors
digitalWrite(encoderPinA, HIGH);
digitalWrite(encoderPinB, HIGH);
digitalWrite(clearButton, HIGH);
attachInterrupt(0, doEncoderB, RISING); //CHANGE
}
void loop()
{
angle = analogRead(sensorPin);
if ((angle < 300) || (angle > 700))
{
md.setM1Speed(0);
md.setM2Speed(0);
}
else if ((current_1 < 6000) || (current_2 < 6000) )
{
//PID
oldangle_2 = oldangle_1;
oldangle_1 = oldangle;
oldangle = newangle;
newangle = (setpointang - angle);
deriv = ((newangle + (3oldangle) -(3oldangle_1) - oldangle_2)(freq/6));
error = (Kp(newangle) + Kd*(deriv));
//Translating PID output value to a value within the range of the motor driver
motorspeed = ((error/511)*400);
//Driving the motors
md.setM1Speed(motorspeed);
md.setM2Speed(motorspeed);
//---------------------------------------------------------------------------------------------------------------------------------------------------------------Angle Control End
//Serial.println(analogRead(A3)); //to plot
if (lastReportedPos != encoderPos) {
if (motorspeed < 0)
{
errorpos = errorpos - (lastReportedPos - encoderPos);
}
else if (motorspeed > 0)
{
errorpos = errorpos + (lastReportedPos - encoderPos);
}
lastReportedPos = encoderPos;
}
errorposctrl = errorpos;
Serial.print(" errorposctrl: ");
Serial.print(errorposctrl);
float a = (errorposctrl/posScaleA);
float b = (errorposctrl/posScaleB);
float c = (errorposctrl/posScaleC);
if (abs(errorposctrl) > ZoneA)
{
setpointang = setpointang - (a);
}
else if (abs(errorposctrl) > ZoneB)
{
setpointang = setpointang - (b);
}
else
{
setpointang = setpointang - (c);
}
Serial.print(" a: ");
Serial.print(a);
Serial.print(" b: ");
Serial.print(b);
Serial.print(" c: ");
Serial.println(c);
//
current_1 = (analogRead(A0)) * 34;
current_2 = (analogRead(A1)) * 34;
//Serial.print(" errorpos: ");
//Serial.print(errorpos);
//Serial.print(" encoderPos: ");
//Serial.println(encoderPos);
//delay((int)T);
}
}
//// Interrupt on A changing state
//void doEncoderA(){
// encoderPos += 1;
//}
// Interrupt on B changing state, same as A above
void doEncoderB()
{
encoderPos += 1;
}
Thanks in advance, MAAhelp
It seems that the problem I am currently encountering could be easily solved by just adding the integral term with regards to the PID control I am currently trying. Does this make sense?