/**************pixy-activitybot-2a.c*********************************************************************************************************************
Tom Montemarano 2014 modified by Dennis van Elteren
Drives activitybot towards defined color signature 1
Uses Pixy interface mode 3 -- voltage corresponding to x-position of largest object
DVE: from the Pixy wiki page: 3: analog/digital x - this will output the x value of the largest
detected object as an analog value between 0 and 3.3V (pin 3). It also outputs whether an object is detected
or not as a digital signal (pin 1 of the I/O connector).
Connections:
Pixypin Activitybotpin
1 adc0
2 +5v
3 adc1
6 gnd
Color needs to be unique and large enough to see at a distance. Laser pointer dot is too small.
ActivityBot speed is kept slow.
DVE additions/changes:
-control of object detection and approach is arranged with a PID algorithm
-the PING sensor is added and protects the distance to objects with a treshold of x Centimeters.
This project runs in a 2nd COG (Propeller has 8 cores aka COGS)
INFO to keep in mind
drive_speed positive values make Abot's wheel(s) go forward, negative = backward
*********************************************************************************************************************************************************/
#include "simpletools.h" // Include simpletools
#include "propeller.h" // for the use of multiple COG's
#include "abdrive.h"
#include "adcDCpropab.h" // Include adcDCpropab
#include "ping.h" // Include ping header
#include
#define rspd 3 // speed to rotate L or R when searching for color
#define fspd 50 // forward speed
#define sspd 30 //search speed
#define treshold_distance 10 //ping sensor treshold in centimeters
//PID defines, used as index for the error array
#define Current 0 //Array index - current error
#define Accumulator 1 //Array index - accumulated error
#define Previous 2 //Array index - previous error
#define Delta 3 //Array index - the delta error
#define FALSE 0
#define TRUE 1
#define STOP 0
float adc0 = 0;
float adc1 = 0; // Voltage variables
//protect the distance to objects in another COG (processor)
void guard_distance();
volatile unsigned int distance = 5000; // For Ping sensor results, startvalue is one that never will happen
volatile unsigned int obstruction = FALSE;
//PID variables
float SetPoint = 1.65; //With Pixy used in mode-3 it outputs a 0-3.3V signal indicating the X-position of targets, 1.65 is the exact middle point
int Kp = 2; //the proportional constant
int Ki = 10; //the integral constant
int Kd = 2; //the differential constant
float error[4] = {0,0,0,0} ; //the difference between setpoint and pixyXcoord
float pixyXcoord; //0-3.3V output indicating the x-position of a target
float Pterm; //the proportional term
float Iterm; //the Integral term
float Dterm; //the Differential term
float sum_of_pidterms;
int drivesignal; //the signal which drives the wheels
int main()
{
drive_speed(0,0); //stop
pause(100);
adc_init(21, 20, 19, 18); // CS=21, SCL=20, DO=19, DI=18
drive_setRampStep(1); // 4 ticks per 50th of sec is default
// Launch distance measuring and guarding function into another cog (processor).
cog_run(&guard_distance, 30);
while(1)
{
if(obstruction == FALSE)//variable obstruction gets TRUE/FALSE in a 2nd COG using the PING sensor guarding a certain treshold distance
{
//first aquire/find a target object
adc0= adc_volts(0);
while((adc0= adc_volts(0)) < 2.4) //turn clockwise circles (to the right) as long as Pixy pin 1 = low (no object detected)
{
Pterm = 0; Iterm = 0; Dterm = 0; //reset the PID values, they will be determined again once the target object is found
memset(error,0,sizeof(error)); //reset the error values in the array as well
drive_speed(sspd,-sspd);
}
//now the target object has been aquired, approach it using a PID algorithm
//obtain the current "sensor" value (= the X coordinate of the pixy as a 0-3.3V voltage)
pixyXcoord = adc_volts(1);
//Calculate the current error (deviation from the setpoint)
error[Current] = SetPoint - pixyXcoord; //the difference between the value wanted and the value measured (pixyXcoord)
//Calculate proportional term.
Pterm = Kp * error[Current];
//Calculate integral term.
error[Accumulator] = error[Accumulator] + error[Current];
Iterm = 0; //Integral term not used for now
//Iterm = Ki * error[Accumulator];
//Calculate derivative term.
error[Delta] = error[Current] - error[Previous];
Dterm = Kd * error[Delta];
//Calculate output, simple truncate of the sum of the 3 floats is used to integer
drivesignal = (int)(Pterm + Iterm + Dterm);
sum_of_pidterms = Pterm + Iterm + Dterm; //floating point precision intact
//limit the pid sum of terms to +/-128
if(drivesignal < -128)
drivesignal = -128;
if(drivesignal > 128)
drivesignal = 128;
//Save current error to previous error before next iteration.
error[Previous] = error[Current];
/*******************Approach #1***************************************************************************************************************
//Drive the motors, the approach is to keep the left wheel at constant speed and change the course by varying the speed of the right wheel
//a positive error indicates the target object is at the left of the X-center, this must be compensated by turning slightly right
//this means for drive_speed(int left, int right) that the ticks per second of the right wheel must be decreased so the Abot goes right
drive_speed(fspd,(fspd-drivesignal));
*******************END approach #1************************************************************************************************************/
/*******************Approach #2***************************************************************************************************************
// positive error means we need to compensate by moving to the right : drive_speed(+ , -) and a negative means to the left : drive_speed(- , +)
// when there is no error (0) we drive at speed "fspd")
if(sum_of_pidterms > 2.0) //if there still is a slight error
{
drive_speed(drivesignal,-drivesignal);
}
else
{
drive_speed(fspd,fspd); //error within the boundaries, drive straight with a constant speed
}
*******************END approach #2************************************************************************************************************/
/*******************Approach #3***************************************************************************************************************/
// use a fixed forward speed with a small differential superimposed on those values
// a positive error means we need to compensate by moving to the right : drive_speed(fspd + delta ,fspd - delta) and a negative means
// to the left : drive_speed(fspd - delta , fspd + delta)
drive_speed((fspd + drivesignal),(fspd - drivesignal));
/*******************END approach #3************************************************************************************************************/
}
else //when there is an obstruction
{
drive_speed(STOP,STOP); //stop
pause(5000); //wait 3 seconds if the target was found before re-starting
}
}
}
//this function runs in another COG and protects the distance to objects
void guard_distance()
{
int pausetime = 50;
while(1)
{
pause(pausetime); //without this delay the distance measurement goes wrong....
if((distance = ping_cm(8)) < treshold_distance)
{
drive_speed(STOP,STOP); //stop
obstruction = TRUE;
high(26); //LED ON (to debug)
freqout(4, 1000, 3000); // short beep (pin, duration, frequency)
pausetime = 5000; //wait for a while before rechecking the distance
}
else
{
obstruction = FALSE;
low(26); //debugled off
pausetime = 50; //re-check in the while(1) loop after 50mS
}
}
}