Robot Car Version#3: Arduino Code
Used encoder feedback and PID control to continuously correct motor speed.
#include <PID_v1.h>
#include <Adafruit_MotorShield.h>
//--------------------------------
// speed and direction
//--------------------------------
// Options: FORWARD, BACKWARD, RELEASE
#define FORWARD_L BACKWARD
#define BACKWARD_L FORWARD
#define FORWARD_R BACKWARD
#define BACKWARD_R FORWARD
#define LEFT 1
#define RIGHT 2
uint8_t leftDir = FORWARD_L;
uint8_t rightDir = FORWARD_R;
// limit: PWM 40~160
#define MINSPEED (4.8*10/16)
#define MAXSPEED (8*10/7)
#define MINPWM 35
#define MAXPWM 160
#define TICK_PER_CIRCLE 1920
#define ERR_INCH 1
int init_pwm = 40;
//--------------------------------
// hardwear setting: motor & board
//--------------------------------
// TB6612 Mosfet Stepper Motor PCA9685 Standard I2C PWM
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motor1 = AFMS.getMotor(1); // M1: left
Adafruit_DCMotor *motor2 = AFMS.getMotor(2); // M2: right
// ----- Encoder Pins -----
const byte LencoderA = 2; // left D2 (A is green) (Interrupt 0)
const byte LencoderB = 4; // left D4 (B is blue)
const byte RencoderA = 3; // right D3 (Interrupt 1)
const byte RencoderB = 5; // right D5
//--------------------------------
// PID and its ISR
//--------------------------------
// ----- counter Updated by ISR -----
volatile long LencoderCount = 0;
volatile long RencoderCount = 0;
// ----- PID Variables -----
double LtargetCount = 0, LcurrentCount = 0, LoutputPWM = 0;
double RtargetCount = 0, RcurrentCount = 0, RoutputPWM = 0;
// ----- adjust Kp, Ki, Kd based on magnitude of error ----
//target: distance in inch (Error range: ~ 1)
//double Kp = 10.0;
//double Ki = 2, Kd = 0;
//target: count (Error range: ~ 192)
double Kp = 0.5;
double Ki = 0.01, Kd = 0;
// ----- debug -----
uint8_t debug = HIGH;
uint8_t enLeft = HIGH;
uint8_t enRight = HIGH;
// ============================================================================
// ============================================================================
//
//
// code starts here
//
//
// ============================================================================
// ============================================================================
// PID( input, output, input, K,K,K,DIRECT);
PID leftPID(&LcurrentCount, &LoutputPWM, &LtargetCount, Kp, Ki, Kd, DIRECT);
PID rightPID(&RcurrentCount, &RoutputPWM, &RtargetCount, Kp, Ki, Kd, DIRECT);
// ----- ISR -----
void LencoderISR() {
if(enLeft != HIGH) return;
LencoderCount++;
//if (leftDir == FORWARD_L) LencoderCount++;
//else LencoderCount--;
//LencoderCount++;
}
void RencoderISR() {
if(enRight != HIGH) return;
RencoderCount++;
//if (rightDir == FORWARD_R) RencoderCount++;
//else RencoderCount--;
//RencoderCount++;
}
//--------------------------------
// control direction && PWM
//--------------------------------
void setDir_fo(){
leftDir = FORWARD_L; rightDir = FORWARD_R;
motor1->run(leftDir); motor2->run(rightDir);
}
void setDir_ba(){
leftDir = BACKWARD_L; rightDir = BACKWARD_R;
motor1->run(leftDir); motor2->run(rightDir);
}
void setDir_left(){
leftDir = BACKWARD_L; rightDir = FORWARD_R;
motor1->run(leftDir); motor2->run(rightDir);
}
void setDir_right(){
leftDir = FORWARD_L; rightDir = BACKWARD_R;
motor1->run(leftDir); motor2->run(rightDir);
}
void setPWM(int PWM_L, int PWM_R){ // 0~255
if(enLeft == HIGH) motor1->setSpeed(PWM_L);
if(enRight == HIGH) motor2->setSpeed(PWM_R);
}
void stopMotors() {
motor1->setSpeed(0);
motor2->setSpeed(0);
}
//--------------------------------
// debug print
//--------------------------------
void print_debug(int i=0){
return;
Serial.print(i); Serial.print(" ");
Serial.print("L current: "); Serial.print(LcurrentCount);
Serial.print(" || target: "); Serial.print(LtargetCount);
Serial.print(" || R current: "); Serial.print(RcurrentCount);
// Serial.print(" | R target: "); Serial.print(RtargetCount);
Serial.print(" || L PWM next: "); Serial.print(LoutputPWM);
Serial.print(" | R PWM next: "); Serial.println(RoutputPWM);
}
void printA_debug(int i=0){
Serial.print("\n "); Serial.print(" "); // <== different from print_debug
Serial.print("L current: "); Serial.print(LencoderCount); // <== different from print_debug
Serial.print(" || target: "); Serial.print(LtargetCount);
Serial.print(" || R current: "); Serial.print(RencoderCount); // <== different from print_debug
// Serial.print(" | R target: "); Serial.print(RtargetCount);
Serial.print(" || L PWM next: "); Serial.print(LoutputPWM);
Serial.print(" | R PWM next: "); Serial.println(RoutputPWM);
}
void print_k(){
Serial.print("init_pwm: "); Serial.print(init_pwm);
Serial.print(", Kp: "); Serial.print(Kp);
Serial.print(", Ki: "); Serial.print(Ki);
Serial.print(", Kd: "); Serial.println(Kd);
}
//----------------------------------------------------------
// convert between count & distance/angle (based on test)
//----------------------------------------------------------
float count2dist(long count){
// count 19200 ~ 97.5 inch
return float(count * 97.5 / 19200.0);
}
float dist2count(float dist){
return float(dist * 19200.0/97.5);
}
float count2angle(long count){
// count 19200 ~ 2220 (Left), 2230(right)
return float(count * 2225.0 / 19200.0);
}
float angle2count(float angle){
return float(angle * 19200.0/2225.0);
}
// ============================================================================
// ============================================================================
// main functions - for contest:
//
// fo(inch, second) //forward
// r(angle) //right
// l(angle) //left
//
// ============================================================================
// ============================================================================
//--------------------------------------
//--------------------------------------
// turn right/left
//--------------------------------------
//--------------------------------------
void r(float angle){turn(angle, RIGHT); }
void l(float angle){turn(angle, LEFT); }
void turn(float angle, int direction) {
float time_sec = 1.2 * angle / 90;
float time_delta = 0.12; // unit: sec
leftPID.SetSampleTime(time_delta); // related to Kp Ki Kd calculation
rightPID.SetSampleTime(time_delta); // related to Kp Ki Kd calculation
float targetCount_delta = angle2count(angle /time_sec * time_delta);
// print
if(debug == HIGH){
if(direction == LEFT) Serial.println("\nTurning left ...");
if(direction == RIGHT) Serial.println("\nTurning right ...");
Serial.print("angle = "); Serial.print(angle);
Serial.print(", time (s)= "); Serial.print(time_sec);
Serial.print(", time_delta (s)= "); Serial.print(time_delta);
Serial.print(", targetCount_delta (inch)= "); Serial.println(targetCount_delta);
print_k();
}
// Reset
LtargetCount = targetCount_delta;
RtargetCount = targetCount_delta;
noInterrupts();
LencoderCount = 0;
RencoderCount = 0;
interrupts();
// before run
if(direction == LEFT)setDir_left();
else setDir_right();
unsigned long durationMS = (unsigned long)(time_sec * 1000.0);
int i = 0;
unsigned long now = 0;
unsigned long startTime = millis();
static unsigned long lastPIDTime = startTime;
// run .......
setPWM((int)init_pwm , (int)init_pwm + 5);
while (millis() - startTime < durationMS ) { // run for specified time
now = millis();
//>>>>>>>>>>> sample PID >>>>>>>>>>
if (now - lastPIDTime >= time_delta * 1000) {
lastPIDTime = now;
//sample: read count
noInterrupts();
LcurrentCount = LencoderCount;
RcurrentCount = RencoderCount;
interrupts();
//update PWM
leftPID.Compute();
rightPID.Compute();
setPWM((int)LoutputPWM, (int)RoutputPWM);
if(debug == HIGH)print_debug(now - startTime); i++;
//update target
LtargetCount += (targetCount_delta);
RtargetCount += targetCount_delta;
}
//<<<<<<<<<<<<<<<<<<<<<
}
setPWM(0, 0);
// debug print
if(debug == HIGH)Serial.println(now - startTime);
if(debug == HIGH){
delay(500);
print_debug(now - startTime);
}
}
//--------------------------------------
//--------------------------------------
// run forward
//--------------------------------------
//--------------------------------------
void fo(float distance, float time_sec) {
float time_delta;
if(distance/time_sec > MAXSPEED ){
Serial.print("ERROR: specified speed is not in MIN~MAX range. ");
Serial.print("distance = "); Serial.print(distance);
Serial.print(", time (s)= "); Serial.print(time_sec);
Serial.print(", distance/time_sec= "); Serial.println(distance/time_sec);
time_sec = distance / MAXSPEED;
time_delta = ERR_INCH / MAXSPEED;
} else if (distance/time_sec < MINSPEED){
Serial.print("ERROR: specified speed is not in MIN~MAX range. ");
Serial.print("distance = "); Serial.print(distance);
Serial.print(", time (s)= "); Serial.print(time_sec);
Serial.print(", distance/time_sec= "); Serial.println(distance/time_sec);
time_sec = distance / MINSPEED;
time_delta = 0.2;
} else{
time_delta = ERR_INCH * time_sec/distance;
if(time_delta > 0.2)time_delta = 0.2; //max allowed: 0.2sec for sampling
}
leftPID.SetSampleTime(time_delta);
rightPID.SetSampleTime(time_delta);
float targetCount_delta = dist2count(distance /time_sec * time_delta);
// print
if(debug == HIGH){
Serial.println("\nRunning forward ...");
Serial.print("distance = "); Serial.print(distance);
Serial.print(", time (s)= "); Serial.print(time_sec);
Serial.print(", time_delta (s)= "); Serial.print(time_delta);
Serial.print(", targetCount_delta (inch)= "); Serial.println(targetCount_delta);
print_k();
}
// Reset
LtargetCount = targetCount_delta;
RtargetCount = targetCount_delta;
noInterrupts();
LencoderCount = 0;
RencoderCount = 0;
interrupts();
// before run
setDir_fo();
unsigned long durationMS = (unsigned long)(time_sec * 1000.0);
int i = 0;
unsigned long now = 0;
unsigned long startTime = millis(); //if(debug == HIGH)Serial.println(startTime);
static unsigned long lastPIDTime = startTime;
// run .......
setPWM((int)init_pwm , (int)init_pwm + 10); //slower one side
while (millis() - startTime < durationMS ) { // run for specified time
now = millis();
//>>>>>>>>>>> sample PID >>>>>>>>>>
if (now - lastPIDTime >= time_delta * 1000) {
lastPIDTime = now;
//sample: read count
noInterrupts();
LcurrentCount = LencoderCount;
RcurrentCount = RencoderCount;
interrupts();
//update PWM
leftPID.Compute();
rightPID.Compute();
setPWM((int)LoutputPWM, (int)RoutputPWM);
if(debug == HIGH)print_debug(now - startTime); i++;
//update target
LtargetCount += (targetCount_delta);
RtargetCount += targetCount_delta;
}
//<<<<<<<<<<<<<<<<<<<<<
}
setPWM(0, 0);
// debug print
if(debug == HIGH)Serial.println(now - startTime);
if(debug == HIGH){
delay(500);
print_debug(now - startTime);
}
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Arduino: program entry
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// setup()
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void setup() {
Serial.begin(57600);
// Setup encoder inputs
pinMode(LencoderA, INPUT_PULLUP);
pinMode(LencoderB, INPUT_PULLUP);
pinMode(RencoderA, INPUT_PULLUP);
pinMode(RencoderB, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(LencoderA), LencoderISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(RencoderA), RencoderISR, CHANGE);
// Setup motor
if (!AFMS.begin()) {
Serial.println("ERROR: Could not find Adafruit Motor Shield. Check wiring!");
exit (1);
}
// Configure PID
leftPID.SetMode(AUTOMATIC);
leftPID.SetOutputLimits(MINPWM, MAXPWM);
rightPID.SetMode(AUTOMATIC);
rightPID.SetOutputLimits(MINPWM, MAXPWM);
//=========================
// run ...
//=========================
delay(200);
//fo(72,9); delay(200);
fo(24, 6); delay(200);
r(90); delay(200);
fo(12, 3); delay(200);
r(90); delay(200);
fo(18, 6); delay(200);
l(90); delay(200);
fo(12, 3); delay(200);
l(90); delay(200);
stopMotors();
}
// ============================================================================
// LOOP
// ============================================================================
void loop() {
// Empty
}
