Hello everyone,
This is my first post here, so please correct me if I've messed up any part of posting to this forum.
I am having some trouble making a following robot project. I am working on this project with my professor, and I have reached a point where I am stuck. I am going to show the picture of the robot first so that you all can see the parts in their locations before I explain the issues.
The robot is supposed to use the two ultrasonic sensors to gauge distance and maintain a certain distance away from whatever it is following. The point of having 2 sensors is so that the robot can turn when the object turns.
The issue that I am having is that when one motor wants to go one direction while the other motor is either stopped or wanting to go in the other direction, neither motor will work. I think this may be an issue with my motor controlling function.
I am open to any and all suggestions, so please let me know what you think I can do. I am not all to familiar with programming, so there is likely just a simple mistake.
Here is my (somewhat silly) code.
/*
* Following Code V3.0
*
* Nickalaus Clemmer
*
* Updated to include the PID library.
*
*/
//Library inclusion
#include <NewPing.h>
#include <PID_v1.h>
//Pin Number Definitions
#define trigLpin 3 //trigger pin for the left sensor
#define trigRpin 10 //trigger pin for the right sensor
#define echoLpin 2 //echo pin for the left sensor
#define echoRpin 11 //echo pin for the right sensor
#define statpin 13
int inCWpin[2] = {7, 4}; // INCW: Clockwise input
int inCCWpin[2] = {8, 9}; // INCCW: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)
//first number controls motor 1, and the second number controls motor 2
//Variable Definitions
#define maxdistance 200
#define sonarnum 2
double toldist=5;
double minaccep;
double maxaccep;
double distanceL;
double distanceR;
double input;
double output;
double setpoint=25;
#define BRAKEVCC 0
#define CW 1
#define CCW 2
#define BRAKEGND 3
#define CS_THRESHOLD 100
//definitions for the motor controller
//Sensory array setup
NewPing sonar[sonarnum]={
NewPing(trigLpin,echoLpin,maxdistance),
NewPing(trigRpin,echoRpin,maxdistance)
};
//0th sensor is the left sensor and the 1st is the right
//PID setup
PID myPID(&input,&output,&setpoint,2,5,1,DIRECT);
//Functions
void motorOff(int motor)
{
// Initialize braked
for (int i=0; i<2; i++)
{
digitalWrite(inCWpin[i], LOW);
digitalWrite(inCCWpin[i], LOW);
}
analogWrite(pwmpin[motor], 0);
}
void motorGo(uint8_t motor, double dist) {
input=dist;
int direct;
if (dist<setpoint) {
direct=1;
}
else {
direct=0;
}
if (direct ==1) { //backwards
digitalWrite(inCWpin[motor], HIGH);
myPID.SetControllerDirection(DIRECT);
}
else if (direct==0) { //forwards
digitalWrite(inCCWpin[motor], HIGH);
myPID.SetControllerDirection(REVERSE);
}
else {
digitalWrite(inCCWpin[motor], LOW);
}
myPID.Compute();
analogWrite(pwmpin[motor],output);
Serial.print("Input ");
Serial.print(input);
Serial.print(" Setpoint ");
Serial.print(setpoint);
Serial.print(" Direction ");
Serial.print(direct);
Serial.print(" PWM ");
Serial.println(output);
}
void setup() {
//Math
minaccep=setpoint-toldist;
maxaccep=setpoint+toldist;
//Serial Initialization
Serial.begin(9600);
//Pin Initialization
pinMode(statpin,OUTPUT);
for (int i=0; i<2; i++)
{
pinMode(inCWpin[i],OUTPUT);
pinMode(inCCWpin[i],OUTPUT);
pinMode(pwmpin[i],OUTPUT);
}
//setup all digital pins as outputs
//Motor Initialization
for (int i=0; i<2; i++)
{
digitalWrite(inCWpin[i],LOW);
digitalWrite(inCCWpin[i],LOW);
}
//setup motors as intially off
myPID.SetMode(AUTOMATIC);
//turn the PID on
}
void loop() {
for (uint8_t i = 0; i < sonarnum; i++) { // Loop through each sensor and display results.
delay(30); // Wait 30ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
if (i==0){
distanceL=sonar[i].ping_cm();
/*
Serial.print("Left: ");
Serial.print(sonar[i].ping_cm());
Serial.print("cm ");
*/
}
else{
distanceR=sonar[i].ping_cm();
/*
Serial.print("Right: ");
Serial.print(sonar[i].ping_cm());
Serial.print("cm ");
*/
}
}
//this loop calculates the distance bewteen the object and each sensor
/*Serial.print("Left: ");
Serial.print(distanceL);
Serial.print("cm ");
Serial.print("Right: ");
Serial.print(distanceR);
Serial.println("cm");
//For debugging only
*/
if (distanceL>maxaccep | distanceL<minaccep){
myPID.SetControllerDirection(DIRECT);
motorGo(1,distanceL);
delay(1);
}
else {
myPID.SetOutputLimits(0.0, 1.0); // Forces minimum up to 0.0
myPID.SetOutputLimits(-1.0, 0.0); // Forces maximum down to 0.0
myPID.SetOutputLimits(0, 255); // Set the limits back to normal
motorOff(1);
}
//controlling left hand motor
if (distanceR>maxaccep | distanceR<minaccep){
myPID.SetControllerDirection(REVERSE);
motorGo(0,distanceR);
delay(1);
}
else {
myPID.SetOutputLimits(0.0, 1.0); // Forces minimum up to 0.0
myPID.SetOutputLimits(-1.0, 0.0); // Forces maximum down to 0.0
myPID.SetOutputLimits(0, 255); // Set the limits back to normal
motorOff(0);
}
//controlling right hand motor
if ((analogRead(cspin[0]) < CS_THRESHOLD) && (analogRead(cspin[1]) < CS_THRESHOLD))
digitalWrite(statpin, HIGH);
}