Yep, thats what I wanted to do! Thanks! I’ll go test it out this morning :).
Edit: Well, I’m looking at my code (I’ll post a copy soon for anyone who wants), and I’m not quite sure where this should go/how to call it. I was thinking of rather than
goto mainloop;
i’d say
goto topspeed_code;
then in topspeed_code, call ‘mainprogram.’
That makes sense to me, BUT - I have timed loops for turning so I can’t call the routine while it’s excecuting - do i put a copy in the same loop with the turning values?
/*
Author: Joshua Siegel
Revision: 3-28-04, 4:54PM
Purpose: Go through a maze and find and extinguish a candle in the middle of a 15cm-radius white circle on black wood. Return trip optional, not attempted in this version.
What it's supposed to do:
1. If UV is false, go to maze logic (can explain if desired, but its pretty simple).
2. If UV is true, count the number of loops spent on white lines on a black floor. We need to stop on a white circle (filled in), so the counter resets itself to avoid mistaking the line for the circle.
4. If UV is true and the robot is not on the circle, go right for 21 loops, then let it resort to the default action until the white line reads true again. This lets the sensor correct for the candle being on the right, but it will find it even if it is on the left because a left turn is the default action.
5. If UV is true and the robot is on the circle, stop and turn on a fan.
6. Not quite sure what to do if the robot is on the circle, but no UV is present (I was thinking of going to findright?).
Sensors: rc_dig_in01 = front IR, rc_dig_in02 = left IR, rc_dig_in03 = right IR, rc_dig_in_04 = UV detector, rc_dig_in05 is the bottom/white line/circle sensor).
To be added: Speed/voltage function if time is left (the old PBASIC robot had a feature where topspeed was limited and as voltage dropped, the topspeed increased so that physical speed remained the same. This made it much easier to program). Return trip.
Notes: Straight has been replaced with soft left (I realized that in open rooms and not hallways, an all-false situation becomes very likely). Disconnect UV sensor power or 9V's entirely, or else breakers will trip even during 'off' mode. Set sensors up to wall color, not hands, paper, or other objects! White value greatly affects read distance. Also, I tried a right-pulse counter to aid in turning. Same with UV (to help assure there is a real flame). Maze logic over-rides candle if IR = 0 for any of the three sensors.
Known issues: Floorcount doesnt seem to work yet.
Accuracy of current revision: 13/15 coming from right. 0/1 coming from left.
*/
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "printf_lib.h"
/* What about 'until logic?' at a later date? - If left true, front false, turn soft right until right true?*/
/* Hard right until left true, hard left until right true. soft left until right true, soft right until left true. */
unsigned int counter = 0;
unsigned int rightturntime = 350;
unsigned int leftturntime = 150;
unsigned int countloop = 0;
unsigned int countloop2 = 0;
unsigned int whitepulse = 0;
unsigned int foundUV = 0;
unsigned int howmanyright = 0;
unsigned int UVpulsecount = 0;
unsigned int UVpulsetimer = 0;
unsigned int UVread = 1;
static void Setup_Who_Controls_Pwms(int pwmSpec1,int pwmSpec2,int pwmSpec3,int pwmSpec4,int pwmSpec5,int pwmSpec6,int pwmSpec7,int pwmSpec8)
{
txdata.pwm_mask = 0xFF;
if (pwmSpec1 == USER)
txdata.pwm_mask &= 0xFE;
if (pwmSpec2 == USER)
txdata.pwm_mask &= 0xFD;
if (pwmSpec3 == USER)
txdata.pwm_mask &= 0xFB;
if (pwmSpec4 == USER)
txdata.pwm_mask &= 0xF7;
if (pwmSpec5 == USER)
txdata.pwm_mask &= 0xEF;
if (pwmSpec6 == USER)
txdata.pwm_mask &= 0xDF;
if (pwmSpec7 == USER)
txdata.pwm_mask &= 0xBF;
if (pwmSpec8 == USER)
txdata.pwm_mask &= 0x7F;
}
void User_Initialization (void)
{
IO1 = IO2 = IO3 = IO4 = IO5 = INPUT;
Set_Number_of_Analog_Channels(TWO_ANALOG);
rc_dig_out06 = 1;
rc_dig_out07 = rc_dig_out09 = 0;
rc_dig_out11 = rc_dig_out13 = rc_dig_out15 = 0;
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
Setup_Who_Controls_Pwms(MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER);
Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);
Initialize_Serial_Comms();
Putdata(&txdata);
User_Proc_Is_Ready();
}
void Process_Data_From_Master_uP(void)
{
Getdata(&rxdata);
Default_Routine();
Putdata(&txdata);
}
void Default_Routine(void)
{
mainprogram:
Putdata(&txdata);
foundUV = 0;
printf ("Floor: %d
", (int)rc_dig_in05);
UVpulsetimer++;
countloop++;
countloop2++;
if (rc_dig_in04 == 1) {
UVpulsecount++;
}
if (UVpulsetimer >= 30) {
UVpulsetimer = 0;
if (UVpulsecount >= 4) {
UVpulsecount = 0;
UVread = 0;
}
else if (UVpulsecount < 4) {
UVpulsecount = 0;
UVread = 1;
}
}
if (rc_dig_in05 == 0) {
whitepulse++;
}
if (rc_dig_in02 == 0) {
howmanyright++;
/*Which, come to think of it, is actually the left sensor! It is called howmanyright because it tries to deter>=e if the turn co>=g up should be right or left.*/
}
if (countloop >= 450) {
countloop = 0;
whitepulse = 0;
goto mainprogram;
}
if (countloop2 >= 1150) {
countloop2 = 0;
howmanyright = 0;
}
if (UVread == 1) {
goto logic;
}
else if (UVread == 0) {
if (whitepulse <= 100) {
if (foundUV <=25) {
findright:
pwm01 = 254;
pwm02 = 245;
Putdata(&txdata);
foundUV++;
}
if (foundUV >=26) {
goto mainprogram;
}
goto mainprogram;
}
if (whitepulse >= 101 && UVread == 0) {
pwm01 = 127;
pwm02 = 127;
rc_dig_out06 = 0;
}
if (whitepulse >= 101 && UVread == 1) {
goto findright;
}
}
logic:
Putdata(&txdata);
pwm01 = 240;
pwm02 = 1;
if (rc_dig_in01 == 0) {
goto Front_True;
}
if (rc_dig_in01 == 1) {
goto Front_False;
}
Front_True:
if (rc_dig_in02 == 0) {
goto FrontTrueLeftTrue;
}
if (rc_dig_in02 == 1) {
goto FrontTrueLeftFalse;
}
goto mainprogram;
Front_False:
if (rc_dig_in02 == 0) {
goto FrontFalseLeftTrue;
}
if (rc_dig_in02 == 1) {
goto FrontFalseLeftFalse;
}
goto mainprogram;
FrontTrueLeftTrue:
if (rc_dig_in03 == 0) {
goto BackUp;
}
if (rc_dig_in03 == 1) {
goto HardRight;
goto mainprogram;
FrontTrueLeftFalse:
if (rc_dig_in03 == 0) {
goto HardLeft;
}
if (rc_dig_in03 == 1 && howmanyright <= 0) {
goto HardLeft;
}
if (rc_dig_in03 == 1 && howmanyright > 0) {
goto HardRight;
}
goto mainprogram;
FrontFalseLeftTrue:
if (rc_dig_in03 == 0) {
goto Straight;
}
if (rc_dig_in03 == 1) {
goto SoftRight;
}
goto mainprogram;
FrontFalseLeftFalse:
if (rc_dig_in03 == 0) {
goto SoftLeft;
}
if (rc_dig_in03 == 1) {
goto SoftLeft; /*Was straight, then changed to soft left in a mad fit of logic)*/
}
goto mainprogram;
/*pwm01 is reversed*/
BackUp:
printf("Mode: Backing up
");
pwm01 = 1;
pwm02 = 254;
goto mainprogram;
HardRight:
counter++;
if (counter <= rightturntime) {
pwm01 = 254;
pwm02 = 210;
Putdata(&txdata);
printf("Mode: Hard right
");
goto HardRight;
}
else if (counter > rightturntime) {
counter = 0;
goto mainprogram;
}
goto mainprogram;
HardLeft:
counter++;
if (counter <= leftturntime) {
printf("Mode: Hard left
");
pwm01 = 1;
pwm02 = 1;
Putdata(&txdata);
goto HardLeft;
}
else if (counter > leftturntime)
counter = 0;
goto mainprogram;
}
goto mainprogram;
Straight: /*Straight is still used in left true/right true/front false situations.*/
printf("Mode: Straight
");
pwm01 = 240;
pwm02 = 1;
goto mainprogram;
SoftLeft:
printf("Mode: Soft left
");
pwm01 = 240;
pwm02 = 1;
goto mainprogram;
SoftRight:
printf("Mode: Soft right
");
pwm01 = 254; /*254*/
pwm02 = 20; /*20*/
goto mainprogram;
}