Hello everyone, new guy here and I have a few questions about using the CMUcam4 and the Paralax Ping sensor with the Arduino Mega. I'm trying to build a color tracking car using multiple PID Controllers, can't use the library, and I think the CMUcam loop is interfering with the distance control via the ping sensor. Here is my code
#include <CMUcam4.h>
#include <CMUcom4.h>
#include <Ping.h>
#include <AFMotor.h>
#define YUV_MODE true
#define RED_MIN 50 //attempting to track the color red
#define RED_MAX 220
#define GREEN_MIN 5
#define GREEN_MAX 50
#define BLUE_MIN 5
#define BLUE_MAX 50
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 seconds
#define PIXELS_THRESHOLD 10 //The percent of tracked pizels needs to be greater than this
#define CONFIDENCE_THRESHOLD 50 // The Percent of tracked pixels in the bounding box needs to be greater than this
#define NOISE_FILTER_LEVEL 2 //Filter out runs of tracked pizels smaller than this length 0 - 255
CMUcam4 cam(CMUCOM4_SERIAL1);
// Motor Shield Setup
AF_DCMotor motorone(1);
AF_DCMotor motortwo(2);
AF_DCMotor motorthree(3);
AF_DCMotor motorfour(4);
int constspeed = 120;
// Ping Sensor Setup
const int numOfReadings = 10; // number of readings to take/ items in the array
int readings[numOfReadings]; // stores the distance readings in an array
int arrayIndex = 0; // arrayIndex of the current item in the array
int total = 0; // stores the cumlative total
int averageDistance = 0; // stores the average value
// setup pins and variables for SRF05 sonar device
int targetdistance = 55;
const int pingpin = 53; // ping pin (digital 53)
unsigned long pulseTime = 0; // stores the pulse in Micro Seconds
unsigned long distance = 0; // variable for storing the distance (cm)
int Kd = 2; //Derivative Gain
int Kp= 1; // propotional Gain
int Kdt = 2; //need to change
int Kpt = 3; // need to change
unsigned long startTime= 0; //keeps track of loop times
void setup(){
Serial.begin(9600);
cam.begin();
// Wait for auto gain and auto white balance to run.
cam.LEDOn(LED_BLINK);
delay(WAIT_TIME);
// Turn auto gain and auto white balance off.
cam.autoGainControl(true);
cam.autoWhiteBalance(true);
cam.LEDOn(CMUCAM4_LED_ON);
cam.colorTracking(YUV_MODE);
cam.noiseFilter(NOISE_FILTER_LEVEL);
//motor setup
motorone.setSpeed(120);
motortwo.setSpeed(120);
motorthree.setSpeed(120);
motorfour.setSpeed(120);
//ping sensor setup
pinMode(pingpin, OUTPUT);
pinMode(pingpin, INPUT);
for (int thisReading = 0; thisReading < numOfReadings; thisReading++){
}
}
void forward(){
motorone.run(FORWARD);
motortwo.run(BACKWARD);
motorthree.run(FORWARD);
motorfour.run(BACKWARD);
}
void backward(){
motorone.run(BACKWARD);
motortwo.run(FORWARD);
motorthree.run(BACKWARD);
motorfour.run(FORWARD);
}
void circle(){
motorone.run(BACKWARD);
motortwo.run(BACKWARD);
motorthree.run(BACKWARD);
motorfour.run(BACKWARD);
}
void loop(){
unsigned long loopTime = millis() - startTime; // time length of each loop
pinMode(pingpin, OUTPUT);
digitalWrite(pingpin, HIGH); // send 10 microsecond pulse
delayMicroseconds(10); // wait 10 microseconds before turning off
digitalWrite(pingpin, LOW); // stop sending the pulse
pinMode(pingpin, INPUT);
pulseTime = pulseIn(pingpin, HIGH); // Look for a return pulse, it should be high as the pulse goes low-high-low
distance = pulseTime/58; // Distance = pulse time / 58 to convert to cm.
total= total - readings[arrayIndex]; // subtract the last distance
readings[arrayIndex] = distance; // add distance reading to array
total= total + readings[arrayIndex]; // add the reading to the total
arrayIndex = arrayIndex + 1; // go to the next item in the array
// At the end of the array (10 items) then start again
if (arrayIndex >= numOfReadings) {
arrayIndex = 0;
}
CMUcam4_tracking_data_t data; // Start streaming
cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX);
for(;;)
{
cam.getTypeTDataPacket(&data); // Get a tracking packet.
int a = data.mx;
if (data.pixels > PIXELS_THRESHOLD) // See the color to track
{
if(data.confidence > CONFIDENCE_THRESHOLD)
{
averageDistance = total / numOfReadings; // calculate the average distance
delay(10);
int P = averageDistance - targetdistance; //position error
int D = P/loopTime; // velocity error
int err = P*Kp + D*Kd;
int motorSpeed = (err/targetdistance*60)+120; //change speed based on object distance
int motorSlow = (err/targetdistance*60)-30; // slow the car if too close to object
if (err < 0){
motorone.run(motorSlow);
motortwo.run(motorSlow);
motorthree.run(motorSlow);
motorfour.run(motorSlow);
backward();
}
if ( err == 0){
int constspeed = motorSpeed+120;
motorone.run(constspeed);
motortwo.run(constspeed);
motorthree.run(constspeed);
motorfour.run(constspeed);
forward();
}
if (err > 0){
motorone.run(motorSpeed);
motortwo.run(motorSpeed);
motorthree.run(motorSpeed);
motorfour.run(motorSpeed);
forward();
}
int desiredPos = 80; //Center CMUcam4 Position
int posErr = a - desiredPos;
int velErr = posErr/loopTime;
int turnErr = Kpt*posErr + Kdt*velErr;
int turnSpeed = turnErr/desiredPos*60 + 60; //turn speed PD control
if (posErr > 80) { //turn RIGHT
motorone.setSpeed(turnSpeed);
motortwo.setSpeed(constspeed);
motorthree.setSpeed(turnSpeed);
motorfour.setSpeed(constspeed);
forward();
}
if(posErr < 80) { //turn LEFT
motortwo.setSpeed(turnSpeed);
motorone.setSpeed(constspeed);
motorfour.setSpeed(turnSpeed);
motorthree.setSpeed(constspeed);
forward();
}
if (posErr == 0){
forward();
}
}
else {
motortwo.setSpeed(constspeed);
motorone.setSpeed(constspeed);
motorfour.setSpeed(constspeed);
motorthree.setSpeed(constspeed);
circle(); //circle to search make the color within the confidence threshold
}
}
else {
motortwo.setSpeed(constspeed);
motorone.setSpeed(constspeed);
motorfour.setSpeed(constspeed);
motorthree.setSpeed(constspeed);
circle(); //circle to search for color
}
}
}
This is the first time I have worked with the CMUcam4 and am a little confused as to some of the nomenclature and how to track objects. the PD control for distance came from a previous project I constructed that worked fine. Anyway, let me know if there is anymore information I can supply. Thank you all for the help!!
Thanks for the replies! When the car is turned on the CMUcam has both lights on, but the ping sensor doesn't activate immediately. After about 10-15 seconds all four wheels start and regardless of color in front of the cam, or proximity to something, it travels forward. It will then randomly stop after some time then repeat the cycle. As I put in the code I put the CMUcam in CMUCOM4_SERIAL1 from what I know that means I can put the TXO of the came in the RX1 port and the RXO of the cam in the TX1 of the arduino mega, It's my first time using the cam.
The structure of the code looks a bit peculiar. In loop() you have an infinite loop, so the first part of loop() where you are measuring the 'time length of each loop' is only done once. If that's your intention, it would be better to put the code that is only intended to run once in setup, the data that needs to be carried across from iteration to iteration as globals, and get rid of the infinite loop in your code; loop() is already called in an infinite loop.
Have you tested the camera and ping sensor input functions and the motor drive functions in isolation? If not, it would be a good idea to test the basics before you try to integrate them together into the overall sketch.
Thanks for the help! The Ping sensor code that had the infinite loop has been removed in favor of using the Ping sensor library, I have also changed around the loops so that the proximity calculations will only work once the object is found. The new code makes the car just circles and doesn't seem to find an object
We have tested everything by itself, the first code I wrote simply made a motor drive forward when a red object was to the left of the camera, stop in the middle, and backward when its to the right.
#include <CMUcam4.h>
#include <CMUcom4.h>
#include <Servo.h>
#include <AFMotor.h>
#include <Ping.h>
#define YUV_MODE false // Change this to true to switch to YUV mode.
#define RED_MIN 230
#define RED_MAX 255
#define GREEN_MIN 230
#define GREEN_MAX 255
#define BLUE_MIN 230
#define BLUE_MAX 255
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 seconds
#define LEFT_LED_PIN 53
#define MIDDLE_LED_PIN 51
#define RIGHT_LED_PIN 49
#define PIXELS_THRESHOLD 0 // The percent of tracked pixels needs to be greater than this 0=0% - 255=100%.
#define CONFIDENCE_THRESHOLD 50 // The percent of tracked pixels in the bounding box needs to be greater than this 0=0% - 255=100%.
#define NOISE_FILTER_LEVEL 2 // Filter out runs of tracked pixels smaller than this in length 0 - 255.
CMUcam4 cam(CMUCOM4_SERIAL1);
AF_DCMotor motor1(1); //Left front motor
AF_DCMotor motor2(2); //Right front motor
AF_DCMotor motor3(3); //Left rear motor
AF_DCMotor motor4(4); //right rear motor
//Ping Pin Setup
const int pingpin = 53;
unsigned long startTime = 0;
int followDistance = 55;
//Controller Values
int Kp = 1; //Proportional Gain
int Kd = 1; //Derivative Gain
void setup()
{
Serial.begin(9600);
cam.begin();
cam.LEDOn(LED_BLINK);
delay(WAIT_TIME);
cam.autoGainControl(false);
cam.autoWhiteBalance(false);
cam.LEDOn(CMUCAM4_LED_ON);
cam.colorTracking(YUV_MODE);
pinMode(LEFT_LED_PIN, OUTPUT);
pinMode(MIDDLE_LED_PIN, OUTPUT);
pinMode(RIGHT_LED_PIN, OUTPUT);
cam.noiseFilter(NOISE_FILTER_LEVEL);
//Motor setup
motor1.setSpeed(120);
motor1.run(RELEASE);
//Ping Sensor setup
pinMode(pingpin, OUTPUT);
pinMode(pingpin, INPUT);
}
void CHARGE(){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void CIRCLE(){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
void loop(){
unsigned long loopTime = millis() - startTime; // tracking loop time
int PulseTime = pulseIn(pingpin, HIGH);
int distance = PulseTime/58; // converts pulseTime to cm
CMUcam4_tracking_data_t data;
cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX);
for(;;){
cam.getTypeTDataPacket(&data); // Get a tracking packet at 30 FPS or every 33.33 ms.
int P = distance - followDistance; //Proportional Error
int D = P/loopTime; //Derivative Error
int PosErr = P*Kp+D*Kd; //Ping Positiong Error
int SpeedControl = (PosErr/followDistance); //Sets forward vehicle speed
//Speeds motor when too far away
if(data.pixels > PIXELS_THRESHOLD){
if(data.confidence > CONFIDENCE_THRESHOLD){
if (PosErr >= 0){
int PingSpeed = 120*(1+SpeedControl);
if (data.mx < 80){
int constSpeed = 120;
int RightSpeed = ((data.mx - 80)/80)*120; // at max right move motor speed double current speed
int TurnSpeed = PingSpeed + RightSpeed;
motor1.setSpeed(TurnSpeed); //faster
motor2.setSpeed(constSpeed);
motor3.setSpeed(TurnSpeed); //faster
motor4.setSpeed(constSpeed);
CHARGE();
}
else if(data.mx == 80){
int constSpeed = 120;
motor1.setSpeed(constSpeed); //faster
motor2.setSpeed(constSpeed);
motor3.setSpeed(constSpeed); //faster
motor4.setSpeed(constSpeed);
CHARGE();
}
else if(data.mx > 0){
int constSpeed = 120;
int RightSpeed = ((data.mx - 80)/80)*120; // at max right move motor speed double current speed
int TurnSpeed = PingSpeed + RightSpeed;
motor1.setSpeed(constSpeed);
motor2.setSpeed(TurnSpeed); //faster
motor3.setSpeed(constSpeed);
motor4.setSpeed(TurnSpeed);//faster
CHARGE();
}
Serial.println(data.mx);
}
else if(PosErr < 0){
int PingSpeed = 120*(1-SpeedControl);
if (data.mx < 80){
int constSpeed = 120;
int RightSpeed = ((data.mx - 80)/80)*120; // at max right move motor speed double current speed
int TurnSpeed = PingSpeed + RightSpeed;
motor1.setSpeed(TurnSpeed); //faster
motor2.setSpeed(constSpeed);
motor3.setSpeed(TurnSpeed); //faster
motor4.setSpeed(constSpeed);
CHARGE();
}
else if(data.mx == 80){
int constSpeed = 120;
motor1.setSpeed(constSpeed); //faster
motor2.setSpeed(constSpeed);
motor3.setSpeed(constSpeed); //faster
motor4.setSpeed(constSpeed);
CHARGE();
}
else if(data.mx > 0){
int constSpeed = 120;
int RightSpeed = ((data.mx - 80)/80)*120; // at max right move motor speed double current speed
int TurnSpeed = PingSpeed + RightSpeed;
motor1.setSpeed(constSpeed);
motor2.setSpeed(TurnSpeed); //faster
motor3.setSpeed(constSpeed);
motor4.setSpeed(TurnSpeed);//faster
CHARGE();
}
Serial.println(data.mx);
}
else{
analogWrite(LEFT_LED_PIN, 0);
analogWrite(MIDDLE_LED_PIN, 0);
analogWrite(RIGHT_LED_PIN, 0);
CIRCLE();
}
}
else{
analogWrite(LEFT_LED_PIN, 0);
analogWrite(MIDDLE_LED_PIN, 0);
analogWrite(RIGHT_LED_PIN, 0);
CIRCLE();
}
}
}
}
Well, it only circles, it never seems to find a target even when we put it right in front of the camera. Some times just wheels 1 and 4 rotate counter to each other while the others are stationary. The ping sensor also never seems to react to anything. Can I use a PD controller with the camera? For some reason my teammate thinks the pd calculations may bee too slow.