Aloha –
That last post of code? It did not fly when dropped into the larger sketch for the bot. Motor speed ramps up, cycles over and over. Other sensor functions are performed, but bot just yo-yo’s along when driving straight. The uncommented Forward() is not the desired option.
Any suggestions, advice would be greatly appreciated, as have been trying to get this, with little success.
Thanks
[code]/*
AStarMiniZumo6x2SpeedRamp.ino
*/
#include <NewPing.h>
/PING def*/
#define TRIGPIN A0
#define ECHOPIN A1
#define MAXDIST 500 // distances in cm
#define COLLDIST 10
#define ALERTDIST 15
//#define ALERTDIST collDist*3
NewPing sonar(TRIGPIN, ECHOPIN, MAXDIST); // sets up sensor library to use the correct pins to measure distance.
/* TB6612FNG and A pins*** */
#define PWMA 10
#define AIN1 7
#define AIN2 8
#define PWMB 11
#define BIN1 6
#define BIN2 5
#define STBY 12
#define MAXSPEED 240
//int speedSet = 0;
int led = 13;
//int sensorPower = 4;
//int speakerPin = 4;
int d = 0; // turn delay variable
int curDist = 0;
int val[3];
//const int numReadings = 3;
/*
const int numReadings = 10;
int readings[numReadings]; // the readings from the analog input
int index = 0; // the index of the current reading
int total = 0; // the running total
int average = 0; // the average
*/
int inputPin = A4;
int potValue = 0;
int readPing() { // read the ultrasonic sensor distance
delay(50);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//**************************** SETUP ****************************************
//***************************************************************************
void setup() {
Serial.begin(9600);
pinMode(led, OUTPUT);
val[0] = 0;
val[1] = 0;
val[2] = 0;
// for (int thisReading = 0; thisReading < numReadings; thisReading++)
// readings[thisReading] = 0;
pinMode(PWMA,OUTPUT);
pinMode(AIN1,OUTPUT);
pinMode(AIN2,OUTPUT);
pinMode(PWMB,OUTPUT);
pinMode(BIN1,OUTPUT);
pinMode(BIN2,OUTPUT);
pinMode(STBY,OUTPUT);
startUp();
setupBlink();
//Forward();
}
//************************ LOOP *********************************************
//***************************************************************************
void loop() {
Forward();
Ping(); // removed the contents in Ping() from here
}
//************************ FUNCTIONS ****************************************
//***************************************************************************
void Ping(){ // Removed from main loop to here
curDist = readPing();
//delay(10);
if (curDist < ALERTDIST)
{
if (curDist < COLLDIST)
{
stopAll();
delay(50);
Backward();
delay(300);
newPath();
}
else
{
newPath();
}
}
else Forward();
}
void newPath()
{
stopAll();
delay(50);
if (random(2) == 0)
{
rotateLeft();
d = ((random(4) + 1) * 100);
delay(d);
senseLeft();
}
else rotateRight();
d = ((random(4) + 1) * 100);
delay(d);
senseRight();
}
void senseLeft()
{
curDist = readPing(); // read distance
if ( curDist < COLLDIST )
{
rotateLeft();
delay(400);
}
else Forward();
}
void senseRight()
{
curDist = readPing(); // read distance
if ( curDist < COLLDIST )
{
rotateRight();
delay(400);
}
else Forward();
}
void checkLR() // Runs in the Forward() funxn
{
int leftValue = analogRead(A2); // LED/phototransistor
int rightValue = analogRead(A3);
if (rightValue < 500 && leftValue < 500)
{
stopAll();
sensorBlink();
delay(50);
sensorBlink();
Backward();
delay(400);
stopAll();
delay(100);
newPath();
}
if (leftValue < 900)
{
stopAll();
sensorBlink();
rotateRight();
delay(100);
}
if (rightValue < 900)
{
stopAll();
sensorBlink();
rotateLeft();
delay(100);
}
}
//**************************** MOTOR CONTROL ********************************
//***************************************************************************
void Forward () // Hard start and stop
{
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15);
digitalWrite (AIN1,HIGH);
digitalWrite (AIN2,LOW);
analogWrite(PWMA,MAXSPEED + potValue); // Offset for speed diff of motors
digitalWrite (BIN1,HIGH);
digitalWrite (BIN2,LOW);
analogWrite(PWMB,MAXSPEED);
delay(20);
checkLR();
}
//**************** Problematic code here *********
/*
void Forward(){ // Soft start, and proceeds like a yo-yo.
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15); // mis-matched motors
digitalWrite (AIN1,HIGH);
digitalWrite (AIN2,LOW);
digitalWrite (BIN1,HIGH);
digitalWrite (BIN2,LOW);
for (int speedSet = 0; speedSet <= MAXSPEED; speedSet +=2)
{
analogWrite(PWMA, speedSet + potValue);
analogWrite(PWMB, speedSet);
if (speedSet >= MAXSPEED){
break;
} // http://arduino.cc/en/Reference/Break gawfawnit!
delay(5);
//Serial.println(speedSet); // when do wheels turn? double that for MINSPEED
} // wheels turn real early, when PWM gets up to 20~30
}
void Forward(){ // Soft start, but drives like a yo-yo
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15);
digitalWrite (AIN1,HIGH);
digitalWrite (AIN2,LOW);
digitalWrite (BIN1,HIGH);
digitalWrite (BIN2,LOW);
for (int speed = 0; speed <= MAXSPEED; speed ++)
{
analogWrite(PWMA, speed + potValue);
analogWrite(PWMB, speed);
delay(5);
}
}
*/
void Backward ()
{
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15);
digitalWrite (AIN1,LOW);
digitalWrite (AIN2,HIGH);
analogWrite(PWMA,MAXSPEED + potValue);
digitalWrite (BIN1,LOW);
digitalWrite (BIN2,HIGH);
analogWrite(PWMB,MAXSPEED);
}
void rotateLeft ()
{
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15);
digitalWrite (AIN1,HIGH);
digitalWrite (AIN2,LOW);
analogWrite(PWMA,(MAXSPEED + potValue) * 0.9);
digitalWrite (BIN1,LOW);
digitalWrite (BIN2,HIGH);
analogWrite(PWMB,MAXSPEED * 0.8);
}
void rotateRight ()
{
potValue = analogRead(A5);
potValue = map(potValue, 0, 1023, 15, -15);
digitalWrite (AIN1,LOW);
digitalWrite (AIN2,HIGH);
analogWrite(PWMA,(MAXSPEED + potValue) * 0.9);
digitalWrite (BIN1,HIGH);
digitalWrite (BIN2,LOW);
analogWrite(PWMB,MAXSPEED * 0.8);
}
void stopAll ()
{
digitalWrite (AIN1,HIGH);
digitalWrite (AIN2,HIGH);
analogWrite(PWMA,255);
digitalWrite (BIN1,HIGH);
digitalWrite (BIN2,HIGH);
analogWrite(PWMB,255);
}
void turnAround()
{
rotateLeft();
delay(1500);
}
void startUp ()
{
digitalWrite(STBY,HIGH);
}
void shutDown ()
{
digitalWrite(STBY,LOW);
}
/*********LED ********* edit to use for (increment) statements and millis() here */
void sensorBlink()
{
digitalWrite(led, HIGH);
delay(50);
digitalWrite(led, LOW);
}
void setupBlink()
{
digitalWrite(led, HIGH);
delay(50);
digitalWrite(led, LOW);
delay(50);
digitalWrite(led, HIGH);
delay(50);
digitalWrite(led, LOW);
}
/*
//************************* PIEZO TONES *************************************
//***************************************************************************
void setupTones(){
playTone(100,1000);
delay(1000);
playTone(100,1300);
delay(1000);
playTone(100,1600);
delay(1000);
}
// duration in mSecs, frequency in hertz
void playTone(long duration, int freq) {
duration *= 1000;
int period = (1.0 / freq) * 1000000;
long elapsed_time = 0;
while (elapsed_time < duration) {
//digitalWrite(speakerPin,HIGH);
delayMicroseconds(period / 2);
//digitalWrite(speakerPin, LOW);
delayMicroseconds(period / 2);
elapsed_time += (period);
}
}
*/[/code]
