code question for self driving truck
gennarobasso81
Posts: 84
hello all,
I am having a hard time figuring out why i cant get a piece of code to work. any ideas would be welcomed.
quick sum up. I took apart my kids rc ford f150. two dc motors control the toy. using prop Activity Board, ping sensor, and two ir leds with detectors; i am trying to make a self driving car.
problem: everything seems to work except the motor control for steering. i wrote functions for everything and am running all sensors in their own cog.
i cute most of the code out to make it easier to read...but the section i am having trouble with is here.
the motor just spins continually. it should turn a little to left or right depending on the ir sensor...then self correct to start position once sensors are cleared of walls.
thanks in advance for looking at it
I am having a hard time figuring out why i cant get a piece of code to work. any ideas would be welcomed.
quick sum up. I took apart my kids rc ford f150. two dc motors control the toy. using prop Activity Board, ping sensor, and two ir leds with detectors; i am trying to make a self driving car.
problem: everything seems to work except the motor control for steering. i wrote functions for everything and am running all sensors in their own cog.
i cute most of the code out to make it easier to read...but the section i am having trouble with is here.
the motor just spins continually. it should turn a little to left or right depending on the ir sensor...then self correct to start position once sensors are cleared of walls.
thanks in advance for looking at it
#include "simpletools.h" // Include simpletools header
#include "ping.h" // Include ping header
#include "abdrive.h"
int pdar(void);
int irLeft(void);
int irRight(void);
void printer(void);
//void mcDrive(void);
void mcSteer(void);
//void backup(void);
unsigned int stack1[40 + 25];
int main() // Main function
{
cogstart(pdar, NULL, stack1, sizeof(stack1)); // Launch ping function into another cog
cog_run(mcSteer, 128); // launch steering into cog
//cog_run(mcDrive, 128);
cog_run(irLeft, 128);
cog_run(irRight, 128);
printer(); // running this function on main cog
}
int pdar(void)
{
while(1) // Repeat indefinitely
{
int cmDist = ping_cm(15); // Get cm distance from Ping)))
pause(200); // Wait 1/5 second
return cmDist;
}
}
int irLeft(void)
{
while(1)
{
int irLeft; // variable
int irPin = 9;
int detPin = 8;
freqout(irPin, 1, 38000); // Left IR LED light
irLeft = input(detPin); // Check left IR detector
return irLeft;
}
}
int irRight(void)
{
while(1)
{
int irRight; // variable
int irPin = 12;
int detPin = 11;
freqout(irPin, 1, 38000); // Right IR LED light
irRight = input(detPin); // Check right IR detector
return irRight;
}
}
void printer(void)
{
while(1)
{
int p = pdar();
int irR = irRight();
int irL = irLeft();
print("%c C.M. from Object = %d\n\n pirLeft = %d, irRight = %d",
HOME, p, irL, irR);
}
}
void mcSteer(void) // stearing wheel command set
{
set_outputs(5, 2, 0b0000);
set_directions(5, 2, 0b1111);
pwm_start(1000);
int p = pdar();
int irL = irLeft();
int irR = irRight();
int right = 4;
int left = 5;
int turnSpeed = 100;
int turnTime = 200;
//(pin, pwm channel, 800/1000)
while(1)
{
if(irL == 0 && irR == 0 && p > 0 && p <= 10)
{
do
{
pause(500); // Stops any motor control for 1/2 second while
}while(irL == 0 && irR == 0 && p > 0 && p <= 25); // in use by mcDrive function.
}
if(irL == 0 && irR == 1)
{
int i = 0;
do
{
do
{
i++;
pwm_set(right, 1,turnSpeed); // turn right
} while(i < turnTime);
}while(irL==0);
if(irL == 1)
{
int i = 0;
do
{
i++;
pwm_set(left, 1,turnSpeed);
} while(i < turnTime);
}
}
else if(irR == 0 && irL == 1)
{
int i = 0;
do
{
do
{
i++;
pwm_set(left, 1,turnSpeed); // turn left
} while(i < turnTime);
}while(irL==0);
if(irL == 1)
{
int i = 0;
do
{
i++;
pwm_set(right, 1,turnSpeed);
} while(i < turnTime);
}
}
}
}
Comments
#include "simpletools.h" // Include simpletools header #include "ping.h" // Include ping header void pdar (void); void irLeft (void); void irRight (void); void printer (void); void mcSteer (void); volatile int cmDist; volatile int irLeft; volatile int irRight; int main () { cog_run(pdar, 128); // Launch ping function into another cog cog_run(mcSteer, 128); // launch steering into cog cog_run(irLeft, 128); cog_run(irRight, 128); printer(); // running this function on main cog } int pdar (void) { while (1) // Repeat indefinitely { cmDist = ping_cm(15); // Get cm distance from Ping))) pause(200); // Wait 1/5 second } } int irLeft (void) { int irPin = 9; int detPin = 8; while (1) { freqout(irPin, 1, 38000); // Left IR LED light irLeft = input(detPin); // Check left IR detector } } int irRight (void) { int irPin = 12; int detPin = 11; while (1) { freqout(irPin, 1, 38000); // Right IR LED light irRight = input(detPin); // Check right IR detector } } void printer (void) { while (1) { int p = pdar(); int irR = irRight(); int irL = irLeft(); print("%c C.M. from Object = %d\n\n pirLeft = %d, irRight = %d", HOME, p, irL, irR); } } /** * stearing wheel command set */ void mcSteer (void) { set_outputs(5, 2, 0b0000); set_directions(5, 2, 0b1111); pwm_start(1000); int right = 4; int left = 5; int turnSpeed = 100; int turnTime = 200; //(pin, pwm channel, 800/1000) while (1) { if (irLeft == 0 && irRight == 0 && cmDist > 0 && cmDist <= 10) { do { pause(500); // Stops any motor control for 1/2 second while } while (irLeft == 0 && irRight == 0 && cmDist > 0 && cmDist <= 25); // in use by mcDrive function. } if (irLeft == 0 && irRight == 1) { int i = 0; do { do { i++; pwm_set(right, 1, turnSpeed); // turn right } while (i < turnTime); } while (irLeft == 0); if (irLeft == 1) { int i = 0; do { i++; pwm_set(left, 1, turnSpeed); } while (i < turnTime); } } else if (irRight == 0 && irLeft == 1) { int i = 0; do { do { i++; pwm_set(left, 1, turnSpeed); // turn left } while (i < turnTime); } while (irLeft == 0); if (irLeft == 1) { int i = 0; do { i++; pwm_set(right, 1, turnSpeed); } while (i < turnTime); } } } }I also removed your invocation to cogstart and replaced it with another cog_run call. No need to combine both cog_run and cogstart - pick one or the other. cog_run is easier, but requires more code space. But if you're already using it once, there's no harm in using it again, so you might as well since it is easier.
Here is the entire program i wrote...thanks again for the help. I have to rebuild the project....but i'll get there in a day or two. i'll post some pics
/* Self driving vehicular robot */ #include "simpletools.h" // Include simpletools header #include "ping.h" // Include ping header #include "abdrive.h" void pdar(void); void leftIR(void); void rigtIR(void); void printer(void); void light(void); void mcDrive(void); void mcSteer(void); void backup(void); volatile int cmDist; volatile int irLeft; volatile int irRight; int main() // Main function { cog_run(pdar, 128); // Launch ping function into another cog cog_run(mcSteer, 128); // launch light function into a third cog cog_run(mcDrive, 128); cog_run(irLeft, 128); cog_run(irRight, 128); cog_run(light, 128); printer(); // running this function on main cog } void pdar(void) { while (1) // Repeat indefinitely { cmDist = ping_cm(15); // Get cm distance from Ping))) pause(200); // Wait 1/5 second } } void leftIR(void) { int irPin = 9; int detPin = 8; while (1) { freqout(irPin, 1, 38000); // Left IR LED light irLeft = input(detPin); // Check left IR detector } } void rightIR(void) { int irPin = 12; int detPin = 11; while (1) { freqout(irPin, 1, 38000); // Right IR LED light irRight = input(detPin); // Check right IR detector } } void printer(void) { while(1) { int p = cmDist; int irR = irRight; int irL = irLeft; print("%c C.M. from Object = %d\n\n pirLeft = %d, irRight = %d", HOME, p, irL, irR); } } void light(void) { while(1) { int p = cmDist; int pin = 6; if(p > 0 && p < 5) { high(pin); pause(5); low(pin); pause(5); } else if(p> 5 && p <= 10) { high(pin); pause(10); low(pin); pause(10); } else if(p >10 && p <= 15) { high(pin); pause(20); low(pin); pause(20); } else if(p >15 && p <= 20) { high(pin); pause(40); low(pin); pause(40); } else if(p >20 && p <= 25) { high(pin); pause(80); low(pin); pause(80); } else if(p >25 && p <= 30) { high(pin); pause(150); low(pin); pause(150); } else if(p >30 && p <= 35) { high(pin); pause(200); low(pin); pause(200); } else if(p >35 && p <= 40) { high(pin); pause(250); low(pin); pause(250); } else if(p >40 && p <= 45) { high(pin); pause(300); low(pin); pause(300); } else if(p >45 && p <= 50) { high(pin); pause(350); low(pin); pause(350); } else if(p >50 && p <= 60) { high(pin); pause(400); low(pin); pause(400); } else if(p >60 && p <= 70) { high(pin); pause(500); low(pin); pause(500); } else if(p >70 && p <= 85) { high(pin); pause(1000); low(pin); pause(1000); } else { low(pin); } } } void mcDrive(void) { while ( 1 ) // Repeat this forever or until loss of power { int p = cmDist; int irL = irLeft; int irR = irRight; //int bup = backup(); int forward = 4; int backward = 5; int right = 2; int left = 3; int turnSpeed = 50; int turnTime = 200; // Keep pins low if unused set_outputs(5, 2, 0b0000); set_directions(5, 2, 0b1111); pwm_start(1000); //(pin, pwm channel, 800/1000) // Move forward 80 percent speed (800 of 1000) if(irL == 0 && irR == 0 && p > 0 && p <= 25) { int i = 0; do { i++; pwm_set(backward, 0,25); // back out of corner } while(i < 1000); do { int i = 0; cog_run(backup, 128); do { i++; pwm_set(right, 1,turnSpeed); } while(i < turnTime); }while(irL==0 && irR==0); } if(p > 0 && p < 5) { pwm_set(backward, 0,25); // rear wheel command set } else if(p> 5 && p <= 10) { pwm_set(backward, 0,25); } else if(p >10 && p <= 15) { pwm_set(backward, 0,25); } else if(p >15 && p <= 20) // ----------zero spot--------------- { pwm_set(forward, 0,0); } else if(p >20 && p <= 25) { pwm_set(forward, 0,25); } else if(p >25 && p <= 30) { pwm_set(forward, 0,50); } else if(p >30 && p <= 35) { pwm_set(forward, 0,100); } else if(p >35 && p <= 40) { pwm_set(forward, 0,150); } else { //--------Delete this statement to go faster----------- pwm_set(forward, 0,200); } /* else if(p >40 && p <= 45) { pwm_set(forward, 0,200); } else if(p >45 && p <= 50) { pwm_set(forward, 0,250); } else if(p >50 && p <= 60) { pwm_set(forward, 0,300); } else if(p >60 && p <= 70) { pwm_set(forward, 0,450); } else if(p >70 && p <= 85) { pwm_set(forward, 0,600); } else if(p >85 && p <= 100) { pwm_set(forward, 0,800); } else { pwm_set(forward, 0,1000); } */ } } void mcSteer(void) // stearing wheel command set { set_outputs(5, 2, 0b0000); set_directions(5, 2, 0b1111); pwm_start(1000); int p = cmDist; int irL = irLeft; int irR = irRight; int right = 2; int left = 3; int turnSpeed = 100; int turnTime = 200; //(pin, pwm channel, 800/1000) while(1) { if(irL == 0 && irR == 0 && p > 0 && p <= 25) { do { pause(500); // Stops any motor control for 1/2 second while }while(irL == 0 && irR == 0 && p > 0 && p <= 25); // in use by mcDrive function. } if(irL == 0) { int i = 0; do { do { i++; pwm_set(right, 1,turnSpeed); // turn right } while(i < turnTime); break; }while(irL==0); pwm_set(right, 0,0); if(irL == 1) { int i = 0; do { i++; pwm_set(left, 1,turnSpeed); } while(i < turnTime); } } else if(irR== 0) { int i = 0; do { do { i++; pwm_set(left, 1,turnSpeed); // turn left } while(i < turnTime); }while(irL==0); if(irL == 1) { int i = 0; do { i++; pwm_set(right, 1,turnSpeed); } while(i < turnTime); } } } } void backup(void) { pwm_set(5, 0,25); }