This is AndroRov V3.2
Code is based upon AndroTest V1.3.ino
#define VERSION "Arduino AndroRov_Video V3.0_text - Rover 5 @kas2014"
#include <PID_v1.h>
// V3.2: 16/05/2014 code cleaning, as posted
// V3.1: 30/04/2014 new algorithm for differential control, CurrentOverload >> send message
// V3.0_int: 16/04/2014 send text values
// V2.5_int: 21/03/2014 send integer values Stable version - Archive -
// V2.4: 21/03/2014: code cleaning
// V2.3: 16/03/2014: PID lib Beauregards
// V2.2: 13/03/2014: >> TODO: implement Integral
// V2.1: 07/03/2014: dead man procedure
// V2.0: 04/03/2014: Current overload
// V1.9: battery voltage + motors current aquisition + PID tuning
// V1.8: OddBot control replaced with PID control
// V1.7: pulse filtering (moving average)
// V1.6: OddBot control right Motor
// V1.5: new loop() design OddBot control right Motor
// V1.4: Codage/config joystick
// V1.3: removed SoftwareSerial
// V1.2: Pb communication error >> TODO remove SoftwareSerial
// V1.1: 4 data byte implemented
// V1.0: 6 buttons implemented
/* -----------------------
For Dagu Rover 5 https://www.sparkfun.com/products/10336
and Dagu Driver Board https://www.sparkfun.com/products/11593
Motors output shaft max RPM: 110 (120 no load)
333 pulses/rev output shaft
gearbox 86.8:1 ratio
motor shaft: 9548RPM (10416) 159RPS(174)
1600ms/pulse @ full speed
Motor 1 (Left) Motor 3 (Right)
DIR1 5 DIR3 8
PWM1 6 PWM3 9
Encoder 2 Encoder 3
AMP1 A1 AMP3 A0
------------------------*/
// Arduino pin#0 to TX BlueTooth module, Arduino pin#1 to RX BlueTooth module
#define DIR_L 5 // Left motor forward/backward pin
#define DIR_R 8 // Right motor forward/backward pin
#define PWM_L 6 // Left motor PWM pin
#define PWM_R 9 // Right motor PWM pin
#define encod_L 2 // Left motor encoder
#define encod_R 3 // Right motor encoder
#define Vpin A0 // battery monitoring analog pin
#define Apin_R A1 // motor current monitoring analog pin
#define Apin_L A2 // motor current monitoring analog pin
#define bipPin 10 // buzzer
#define ledPin 13 // on board LED
#define STX 0x01
#define ETX 0x00
#define FW true
#define BW false
#define GUARD_GAIN 20
#define MASK B11111111 // low bit mask
#define NUMREADINGS 100 // smoothing, buffer size
float Kp = 2; // PID Proportional control Gain
float Ki = 2; // PID Integral control Gain
float Kd = 1; // PID Derivative control Gain
int i=0;
byte cmd[6] = {0, 0, 0, 0, 0, 0}; // bytes received
byte buttonStatus = 0; // first Byte sent to Android device
boolean setButtonFeedback = false; // momentary buttons feedback to Android device
long mainLoopInterval = 5000; // Main loop timing (microseconds)
long sendInterval = 500; // interval between Buttons status transmission (milliseconds)
long checkOverloadInterval = 500; // interval between motors overload checks (milliseconds)
long checkLowBatInterval = 2000; // interval between low battery checks (milliseconds)
long deadManInterval = 1100; // interval between low battery checks (milliseconds)
unsigned long mtime; // main loop timing
volatile unsigned long pulse_L = 100000; // width of left and right encoder pulses in uS
volatile unsigned long pulse_R = 100000; // width of left and right encoder pulses in uS
volatile unsigned long time_L = 0; // remembers time of left encoders last state change in uS
volatile unsigned long time_R = 0; // remembers time of right encoders last state change in uS
double pwm_L, pwm_R; // left and right motor PWMs generated from the processor
unsigned long pulseAvg_L, pulseAvg_R; // smoothed encoder data
unsigned long actual = 0; // temporary calculation of actual left and right motor speeds in uS between encoder pulses
double speedRequired_L = 0, speedRequired_R = 0; // joystick requested speed (-100 =100)
double speedActual_L = 0, speedActual_R = 0; // encoder measured motors speed
unsigned long readings_L[NUMREADINGS]; // data buffer for pulse readings - Left motor
unsigned long readings_R[NUMREADINGS]; // data buffer for pulse readings - Right motor
boolean enc_R = false, enc_L = false; // flags for new encoder interrupts
int voltage = 0; // battery voltage in mV X10
int current_L = 0, current_R = 0; // motor current in mA
int currentLimit = 1500; // motor Amp limit (mA)
int voltLimit = 7000; // low battery limit (mV)
boolean overLoad = false; // motors over load status
boolean underVolt = false; // low battery level status
boolean deadManEnabled = true; // stop Rover if BT com is lost
boolean deadManTimeout = false; // dead man flag
String displayStatus = "xxxx"; // message to Android device
PID PID_L(&speedActual_L, &pwm_L, &speedRequired_L,Kp,Ki,Kd, DIRECT);
PID PID_R(&speedActual_R, &pwm_R, &speedRequired_R,Kp,Ki,Kd, DIRECT);
void setup() {
Serial.begin(57600);
pinMode(DIR_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(PWM_L, OUTPUT);
pinMode(PWM_R, OUTPUT);
pinMode(ledPin, OUTPUT);
pinMode(bipPin, OUTPUT);
pinMode(encod_L, INPUT); // Left motor
pinMode(encod_R, INPUT); // Right motor
digitalWrite(encod_L, HIGH); // turn on pullup resistor
digitalWrite(encod_R, HIGH); // turn on pullup resistor
attachInterrupt(0, rencoder_L, CHANGE);
attachInterrupt(1, rencoder_R, CHANGE);
Motor_L((FW), abs(0));
Motor_R((FW), abs(0));
for(int i=0; i<NUMREADINGS; i++) readings_R[i] = 0; // initialize readings to 0
PID_L.SetMode(AUTOMATIC);
PID_R.SetMode(AUTOMATIC);
PID_L.SetSampleTime(10);
PID_R.SetSampleTime(10);
setButtonFeedback = true;
sendBlueToothData(true); // Send data to smartphone
bip(bipPin, 10, 2); // ready
}
void loop() {
if(micros()- mtime >mainLoopInterval) { // 5000 // Call motor control function every 5mS
mtime=micros(); // reset motor timer
int byteNumber = getBlueToothData(); // Get joystick and button data from smartphone
if(byteNumber == 2) getButtonState(cmd[1]); // Process button data
else if(byteNumber == 5) getJoystickState(cmd); // Process joystick data
else if(byteNumber >= 0) bip(bipPin, 10, 2); // Communication error
if(enc_L) { // interrupt Left motor
enc_L = false;
pulseAvg_L = digitalSmooth_L(pulse_L); // filter pulse data
}
if(enc_R) { // interrupt Right motor
enc_R = false;
pulseAvg_R = digitalSmooth_R(pulse_R); // filter pulse data
}
** The message exceeds the maximum allowed length (9500 characters) **
Download attached file
Make sure to download Beauregards PID library V1.01, and install it in your "libraries" directory
Arduino is connected to a Dagu quad driver board
You can also get, on eBay, a dual L298N based controller at a much cheaper price
AndroRov_V32_as_published.ino (19 KB)