Version 1
Unlike Version 2, all separate files of version one are fully functional separate programs requiring separate flashes of the MCU.
PID_V1.ino
pid_v1.ino |
#include <QTRSensors.h>
// Clockwise and counter-clockwise definitions.
// Depending on how you wired your motors, you may need to swap.
#define CW 0
#define CCW 1
// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1
// Pin Assignments
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 10; // PWM control (speed) for motor B
const byte DIRA = 9; // Direction control for motor A
const byte DIRB = 8; // Direction control for motor B
#define NUM_SENSORS 8 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define KP 0.15
#define KD 0.25
#define M1 100
#define M2 M1
// sensors 0 through 7 are connected to digital pins 3 through 10, respectively
QTRSensorsRC qtrrc((unsigned char[]) {
A0, A1, A2, A3, A4, A5, 4, 5
},
NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
Serial.begin(9600);
delay(500);
Serial.println("Calibrating...");
// pinMode(13, OUTPUT);
// digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}
// digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration
// print the calibration minimum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println("Calibration Complete.");
delay(1000);
setupArdumoto();
}
int lastError = 0;
void loop()
{
//Get sensor readings
unsigned int sensors[3];
int position = qtrrc.readLine(sensors);
//Calculate the error
int error = position - 3000;
//Do the PID math
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
//Put motor speed
int m1Speed = M1 + motorSpeed;
int m2Speed = M2 - motorSpeed;
//If the motor speed exceeds the bounds, use max or min values
if (m1Speed < 0)
m1Speed = 0;
if (m2Speed < 0)
m2Speed = 0;
if (m1Speed > 255)
m1Speed = 255;
if (m2Speed > 255)
m2Speed = 255;
//Debug messages
Serial.print("Pos: ");
Serial.print(position);
Serial.print(", Error: ");
Serial.print(motorSpeed);
Serial.print(", m1Speed: ");
Serial.print(m1Speed);
Serial.print(", m2Speed: ");
Serial.println(m2Speed);
// set motor speeds using the two motor speed variables above
driveArdumoto(MOTOR_B, CW, (byte)m1Speed);
driveArdumoto(MOTOR_A, CW, (byte)m2Speed);
}
// driveArdumoto drives 'motor' in 'dir' direction at 'spd' speed
void driveArdumoto(byte motor, byte dir, byte spd)
{
// Serial.print("Motor: ");
// Serial.print(motor);
// Serial.print("\tDir: ");
// Serial.print(dir);
// Serial.print("\tSpd: ");
// Serial.println(spd);
//Motor won't run anyway under 50
if (motor == MOTOR_A)
{
digitalWrite(DIRA, dir > 0 ? HIGH : LOW);
analogWrite(PWMA, spd);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, dir > 0 ? HIGH : LOW);
analogWrite(PWMB, spd);
}
}
// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0, 0);
}
// setupArdumoto initialize all pins
void setupArdumoto()
{
// All pins should be setup as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);
// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}
// Clockwise and counter-clockwise definitions.
// Depending on how you wired your motors, you may need to swap.
#define CW 0
#define CCW 1
// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1
// Pin Assignments
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 10; // PWM control (speed) for motor B
const byte DIRA = 9; // Direction control for motor A
const byte DIRB = 8; // Direction control for motor B
#define NUM_SENSORS 8 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 microseconds for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define KP 0.15
#define KD 0.25
#define M1 100
#define M2 M1
// sensors 0 through 7 are connected to digital pins 3 through 10, respectively
QTRSensorsRC qtrrc((unsigned char[]) {
A0, A1, A2, A3, A4, A5, 4, 5
},
NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
Serial.begin(9600);
delay(500);
Serial.println("Calibrating...");
// pinMode(13, OUTPUT);
// digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}
// digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration
// print the calibration minimum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println("Calibration Complete.");
delay(1000);
setupArdumoto();
}
int lastError = 0;
void loop()
{
//Get sensor readings
unsigned int sensors[3];
int position = qtrrc.readLine(sensors);
//Calculate the error
int error = position - 3000;
//Do the PID math
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
//Put motor speed
int m1Speed = M1 + motorSpeed;
int m2Speed = M2 - motorSpeed;
//If the motor speed exceeds the bounds, use max or min values
if (m1Speed < 0)
m1Speed = 0;
if (m2Speed < 0)
m2Speed = 0;
if (m1Speed > 255)
m1Speed = 255;
if (m2Speed > 255)
m2Speed = 255;
//Debug messages
Serial.print("Pos: ");
Serial.print(position);
Serial.print(", Error: ");
Serial.print(motorSpeed);
Serial.print(", m1Speed: ");
Serial.print(m1Speed);
Serial.print(", m2Speed: ");
Serial.println(m2Speed);
// set motor speeds using the two motor speed variables above
driveArdumoto(MOTOR_B, CW, (byte)m1Speed);
driveArdumoto(MOTOR_A, CW, (byte)m2Speed);
}
// driveArdumoto drives 'motor' in 'dir' direction at 'spd' speed
void driveArdumoto(byte motor, byte dir, byte spd)
{
// Serial.print("Motor: ");
// Serial.print(motor);
// Serial.print("\tDir: ");
// Serial.print(dir);
// Serial.print("\tSpd: ");
// Serial.println(spd);
//Motor won't run anyway under 50
if (motor == MOTOR_A)
{
digitalWrite(DIRA, dir > 0 ? HIGH : LOW);
analogWrite(PWMA, spd);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, dir > 0 ? HIGH : LOW);
analogWrite(PWMB, spd);
}
}
// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0, 0);
}
// setupArdumoto initialize all pins
void setupArdumoto()
{
// All pins should be setup as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);
// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}
Reciever.ino
receiver.ino |
#include <SPI.h>
#include "RF24.h"
// Clockwise and counter-clockwise definitions.
// Depending on how you wired your motors, you may need to swap.
#define CW 0
#define CCW 1
// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1
// Pin Assignments //
// Don't change these! These pins are statically defined by shield layout
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 10; // PWM control (speed) for motor B
const byte DIRA = 9; // Direction control for motor A
const byte DIRB = 8; // Direction control for motor B
#define COMMAND_SIZE 4
RF24 radio(6, 7);
byte TRANSMITTER_ADDRESS[] = "EAKJB_RECEIVER";
byte RECEIVER_ADDRESS[] = "EAKJB_TRANSMITTER";
const bool PIN_OUTPUTS[] = {
false, false, true, true, true, true, true, true, true, true, true, true, true, false
};
const bool PIN_ANALOGS[] = {
false, false, false, true, false, true, true, false, false, true, true, true, false, false
};
void setup() {
//Initialize serial...
Serial.begin(9600);
setupArdumoto(); // Set all pins as outputs
Serial.print("Intializing receiver...");
for (byte i = 0; i < sizeof(PIN_OUTPUTS); i++) {
Serial.print(i);
Serial.print(", ");
pinMode(i, OUTPUT);
}
//Initialize radio...
radio.begin();
//Set power level to low because communication is short-range
radio.setPALevel(RF24_PA_LOW);
//Open reading and writing pipes (addresses)
//Switch the addresses on the transmitter
radio.openWritingPipe(TRANSMITTER_ADDRESS);
radio.openReadingPipe(1, RECEIVER_ADDRESS);
//Start listening
radio.startListening();
//Notify debugger that initialization has finished.
Serial.println("Done.");
}
void loop() {
//Wait for the transmitter to send something
if (radio.available()) {
Serial.print("Receiving command...");
//Initialize an array to hold the command
byte command[COMMAND_SIZE];
//Set to false if there is an error reading transmission
bool okay = true;
radio.read(command, sizeof(byte)*COMMAND_SIZE);
//If the transmission has more than 4 bytes, handle this as an
//error and throw out the additional bytes.
while (radio.available()) {
Serial.println("Error reading command. Too many bytes.");
okay = false;
byte junk;
radio.read(&junk, sizeof(byte));
}
//Give a debug message and print the received command
Serial.print("Done. Received: ");
for (byte i = 0; i < COMMAND_SIZE; i++) {
Serial.print(command[i], DEC);
Serial.print(", ");
}
Serial.println();
//Process the command
processCommand(command);
}
}
void processCommand(byte command[]) {
if (command[0] == 0) { //PING
Serial.println("Ping not implemented yet.");
} else if (command[0] == 1) { //PIN CONTROL
//Check if the pin is valid and allowed
if (command[1] > 0 && command[1] < sizeof(PIN_OUTPUTS) && PIN_OUTPUTS[command[1]]) {
Serial.print("Setting Pin ");
Serial.print(command[1]);
Serial.print(" to ");
//Check if the pin is analog
if (PIN_ANALOGS[command[1]]) {
Serial.print(command[2]);
//If the pin is analog, write the value to it.
analogWrite(command[1], command[2]);
} else {
//If the pin is digital, go HIGH if the desired value is above 0.
if (command[2] > 0) {
Serial.print("HIGH");
digitalWrite(command[1], HIGH);
} else {
Serial.print("LOW");
digitalWrite(command[1], LOW);
}
}
Serial.println(".");
} else {
//The pin is invalid.
Serial.print("Invalid pin: ");
Serial.println(command[1]);
}
} else if (command[0] == 2) {
// set motor speeds using the two motor speed variables above
driveArdumoto(command[1], command[3], command[2]);
}
}
// driveArdumoto drives 'motor' in 'dir' direction at 'spd' speed
void driveArdumoto(byte motor, byte dir, byte spd)
{
Serial.print("Motor: ");
Serial.print(motor);
Serial.print("\tDir: ");
Serial.print(dir);
Serial.print("\tSpd: ");
Serial.println(spd);
//Motor won't run anyway under 50
if (spd > 50) {
if (motor == MOTOR_A)
{
digitalWrite(DIRA, dir>0?HIGH:LOW);
analogWrite(PWMA, spd);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, dir>0?HIGH:LOW);
analogWrite(PWMB, spd);
}
} else {
if (motor == MOTOR_A)
{
digitalWrite(DIRA, 0);
analogWrite(PWMA, 0);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, 0);
analogWrite(PWMB, 0);
}
}
}
// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0, 0);
}
// setupArdumoto initialize all pins
void setupArdumoto()
{
// All pins should be setup as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);
// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}
#include "RF24.h"
// Clockwise and counter-clockwise definitions.
// Depending on how you wired your motors, you may need to swap.
#define CW 0
#define CCW 1
// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1
// Pin Assignments //
// Don't change these! These pins are statically defined by shield layout
const byte PWMA = 3; // PWM control (speed) for motor A
const byte PWMB = 10; // PWM control (speed) for motor B
const byte DIRA = 9; // Direction control for motor A
const byte DIRB = 8; // Direction control for motor B
#define COMMAND_SIZE 4
RF24 radio(6, 7);
byte TRANSMITTER_ADDRESS[] = "EAKJB_RECEIVER";
byte RECEIVER_ADDRESS[] = "EAKJB_TRANSMITTER";
const bool PIN_OUTPUTS[] = {
false, false, true, true, true, true, true, true, true, true, true, true, true, false
};
const bool PIN_ANALOGS[] = {
false, false, false, true, false, true, true, false, false, true, true, true, false, false
};
void setup() {
//Initialize serial...
Serial.begin(9600);
setupArdumoto(); // Set all pins as outputs
Serial.print("Intializing receiver...");
for (byte i = 0; i < sizeof(PIN_OUTPUTS); i++) {
Serial.print(i);
Serial.print(", ");
pinMode(i, OUTPUT);
}
//Initialize radio...
radio.begin();
//Set power level to low because communication is short-range
radio.setPALevel(RF24_PA_LOW);
//Open reading and writing pipes (addresses)
//Switch the addresses on the transmitter
radio.openWritingPipe(TRANSMITTER_ADDRESS);
radio.openReadingPipe(1, RECEIVER_ADDRESS);
//Start listening
radio.startListening();
//Notify debugger that initialization has finished.
Serial.println("Done.");
}
void loop() {
//Wait for the transmitter to send something
if (radio.available()) {
Serial.print("Receiving command...");
//Initialize an array to hold the command
byte command[COMMAND_SIZE];
//Set to false if there is an error reading transmission
bool okay = true;
radio.read(command, sizeof(byte)*COMMAND_SIZE);
//If the transmission has more than 4 bytes, handle this as an
//error and throw out the additional bytes.
while (radio.available()) {
Serial.println("Error reading command. Too many bytes.");
okay = false;
byte junk;
radio.read(&junk, sizeof(byte));
}
//Give a debug message and print the received command
Serial.print("Done. Received: ");
for (byte i = 0; i < COMMAND_SIZE; i++) {
Serial.print(command[i], DEC);
Serial.print(", ");
}
Serial.println();
//Process the command
processCommand(command);
}
}
void processCommand(byte command[]) {
if (command[0] == 0) { //PING
Serial.println("Ping not implemented yet.");
} else if (command[0] == 1) { //PIN CONTROL
//Check if the pin is valid and allowed
if (command[1] > 0 && command[1] < sizeof(PIN_OUTPUTS) && PIN_OUTPUTS[command[1]]) {
Serial.print("Setting Pin ");
Serial.print(command[1]);
Serial.print(" to ");
//Check if the pin is analog
if (PIN_ANALOGS[command[1]]) {
Serial.print(command[2]);
//If the pin is analog, write the value to it.
analogWrite(command[1], command[2]);
} else {
//If the pin is digital, go HIGH if the desired value is above 0.
if (command[2] > 0) {
Serial.print("HIGH");
digitalWrite(command[1], HIGH);
} else {
Serial.print("LOW");
digitalWrite(command[1], LOW);
}
}
Serial.println(".");
} else {
//The pin is invalid.
Serial.print("Invalid pin: ");
Serial.println(command[1]);
}
} else if (command[0] == 2) {
// set motor speeds using the two motor speed variables above
driveArdumoto(command[1], command[3], command[2]);
}
}
// driveArdumoto drives 'motor' in 'dir' direction at 'spd' speed
void driveArdumoto(byte motor, byte dir, byte spd)
{
Serial.print("Motor: ");
Serial.print(motor);
Serial.print("\tDir: ");
Serial.print(dir);
Serial.print("\tSpd: ");
Serial.println(spd);
//Motor won't run anyway under 50
if (spd > 50) {
if (motor == MOTOR_A)
{
digitalWrite(DIRA, dir>0?HIGH:LOW);
analogWrite(PWMA, spd);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, dir>0?HIGH:LOW);
analogWrite(PWMB, spd);
}
} else {
if (motor == MOTOR_A)
{
digitalWrite(DIRA, 0);
analogWrite(PWMA, 0);
}
else if (motor == MOTOR_B)
{
digitalWrite(DIRB, 0);
analogWrite(PWMB, 0);
}
}
}
// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
{
driveArdumoto(motor, 0, 0);
}
// setupArdumoto initialize all pins
void setupArdumoto()
{
// All pins should be setup as outputs:
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRA, OUTPUT);
pinMode(DIRB, OUTPUT);
// Initialize all pins as low:
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(DIRA, LOW);
digitalWrite(DIRB, LOW);
}
Transmitter.ino
Transmitter.ino runs on the remote control's MCU as opposed to the vehicle's MCU.
transmitter.ino |
#include <SPI.h>
#include "RF24.h"
#define COMMAND_SIZE 4
RF24 radio(7, 8);
byte TRANSMITTER_ADDRESS[] = "EAKJB_RECEIVER";
byte RECEIVER_ADDRESS[] = "EAKJB_TRANSMITTER";
void setup() {
//Initialize serial...
Serial.begin(9600);
Serial.print("Intializing transmitter...");
//Initialize radio...
radio.begin();
//Set power level to low becaue communication is short-range
radio.setPALevel(RF24_PA_LOW);
//Open reading and writing pipes (addresses)
//Switch the addresses on the transmitter
radio.openWritingPipe(RECEIVER_ADDRESS);
radio.openReadingPipe(1, TRANSMITTER_ADDRESS);
//Start listening
radio.startListening();
//Notify debugger that initialization has finished.
Serial.println("Done.");
}
bool state = true;
void loop() {
Serial.print("Transmitting command...");
byte command[COMMAND_SIZE] = {0x01, 0x05, state?0xFF:0x33, 0x00};
state = !state;
radio.stopListening();
radio.write(&command, sizeof(byte)*COMMAND_SIZE);
radio.startListening();
Serial.println("Done.");
delay(1000);
}
#include "RF24.h"
#define COMMAND_SIZE 4
RF24 radio(7, 8);
byte TRANSMITTER_ADDRESS[] = "EAKJB_RECEIVER";
byte RECEIVER_ADDRESS[] = "EAKJB_TRANSMITTER";
void setup() {
//Initialize serial...
Serial.begin(9600);
Serial.print("Intializing transmitter...");
//Initialize radio...
radio.begin();
//Set power level to low becaue communication is short-range
radio.setPALevel(RF24_PA_LOW);
//Open reading and writing pipes (addresses)
//Switch the addresses on the transmitter
radio.openWritingPipe(RECEIVER_ADDRESS);
radio.openReadingPipe(1, TRANSMITTER_ADDRESS);
//Start listening
radio.startListening();
//Notify debugger that initialization has finished.
Serial.println("Done.");
}
bool state = true;
void loop() {
Serial.print("Transmitting command...");
byte command[COMMAND_SIZE] = {0x01, 0x05, state?0xFF:0x33, 0x00};
state = !state;
radio.stopListening();
radio.write(&command, sizeof(byte)*COMMAND_SIZE);
radio.startListening();
Serial.println("Done.");
delay(1000);
}