Challenge A
Hoe hebben we onze robot gemonteerd?
Bij challenge A hebben we ervoor gekozen om onze robot uit te rusten met twee ultrasone sensoren, een heben we voorop de robot gemonteerd zodat we de afstand tussen de voorkant van de robot en een object dat zich voor de robot bevind te meten. Aan de rechterkant hebben we ook een ultrasone sensor gemonteerd, deze is bedoeld om de afstand van de rechtermuur van een doolhof tot de rechterkant van de robot te meten.
Naast de ultrasone sensoren die we van onze docent hebben gekregen hebben we ook nog een bumper gemaakt voor de robot. Deze bumper zorgt er niet alleen voor dat de robot niet vast blijft haken, maar werkt tevens als een push switch welke we in onze software uit kunnen lezen om de robot uit lastige situaties te redden.
De code.
#include "simpletools.h"
#include "servo.h"
#include "ping.h"
#include "abdrive.h"
//PARAMETERS
const int rightPingPort = 16;
const int frontPingPort = 2;
const int bumperPort = 6;
const int maxRightTurns = 5;//after so many turns, the robot will reset
const int resetRightTurnsOnState1 = 0;
//VARIABLES
volatile static int checkForBumperPress;
volatile static int bumperPressedOnRightTurn;
volatile static int distance[2];
volatile static int state;
volatile static int escapeCount;
volatile static int leftTurnCount;
volatile static int rightTurnCount;
unsigned int scannerStack[40+25];
unsigned int stateStack[40+25];
const int FRONT = 0;
const int RIGHT = 1;
//DECLARATION OF FUNCTIONS
void updateDistanceCog();
void driveStateCog();
void drive();
void escape();
void stop();
void turnRight();
void rotateRight();
void turnLeft(int iter);
void startup();
void escapeBumper();
//MAIN
int main(){
//initialisation
state = -2;
leftTurnCount = 0;
rightTurnCount = 0;
escapeCount = 0;
checkForBumperPress = 0;
bumperPressedOnRightTurn = 0;
//Start cogs
cogstart(&updateDistanceCog, NULL, scannerStack, sizeof(scannerStack));
cogstart(&driveStateCog, NULL, stateStack, sizeof(stateStack));
pause(2000);
/*
*MAIN PROGRAM LOOP
*/
while(1){
pause(100);
int right = distance[RIGHT];
int front = distance[FRONT];
int bumper = input(bumperPort);
//SENSOR INPUT
if(bumper == 0){
state = 4; //Bumper is pressed
}
else if((right > 15)&&(right < 200)){
state = 2; //Right turn
}
else if(right < 10 && front <= 6){
state = 3; //Left turn
}
else if(front>6){
state = 1; //Drive forward
}
else{
state = -1; //Get some distance
}
//CHECK FOR BUMPER PRESS ON RIGHT TURN
if(checkForBumperPress==1){
if (input(bumperPort)==0){
bumperPressedOnRightTurn=1;
}
}
else{
bumperPressedOnRightTurn = 0;
}
}
return 0;
}
/*
*COG: PINGSENSORLOOP
*/
void updateDistanceCog(){
while(1){
distance[FRONT] = ping_cm(frontPingPort);
pause(6);
distance[RIGHT] = ping_cm(rightPingPort);
pause(6);
}
}
/*
*COG: DRIVESTATECOG
*/
void driveStateCog(){
while(1){
switch(state){
case -2://STARTUP
leftTurnCount=0;
rightTurnCount=0;
state = 0;
startup();
break;
case -1://TOO CLOSE TO THE WALL
escapeCount++;
leftTurnCount = 0;
if(escapeCount<4){
escape();
}else{
high(27);
escapeCount = 0;
pause(10);
drive_goto(1, 1);
drive_goto(-1, -1);
rotateRight();
drive_goto(45, 45);
low(27);
}
state = 0;
break;
case 0://STOP
stop();
break;
case 1://DRIVE FORWARD
leftTurnCount = 0;
if(resetRightTurnsOnState1==1){
rightTurnCount=0;
}
drive();
break;
case 2://TURN RIGHT
leftTurnCount = 0;
turnRight();
rightTurnCount++;
state = 0;
break;
case 3://TURN LEFT
turnLeft(leftTurnCount++);
rightTurnCount=0;
state = 0;
break;
case 4://BUMPER PRESS
escapeBumper();
state = 0;
break;
default:
leftTurnCount = 0;
state = 0;
break;
}
}
}
//robot drives forward
void drive(){
drive_speed(30, 30);
}
//robot drives a bit backwards
void escape(){
high(26);//status led
drive_goto(-5,-5);
low(26);
}
//robot stops driving
void stop(){
drive_speed(0,0);
}
//bumper is pressed
void escapeBumper(){
escape();
drive_goto(-3, 3); //Turn a bit to the left
}
//robot turns 90 degrees to the right
void turnRight(){
//The robot turned too much to the right, and is problably stuck in a loop
//run startup
if((maxRightTurns>0)&&(rightTurnCount>maxRightTurns)){
startup();
leftTurnCount=0;
rightTurnCount=0;
state = 0;
return;
}
drive_goto(18, 18);
rotateRight();
checkForBumperPress=1;//main loop starts to check for a bumper press
drive_goto(45, 45);
//If the bumper is pressed, turn back to the left
if(bumperPressedOnRightTurn==1){
turnLeft(0);
drive_goto(2,2);
rightTurnCount--;//lower the rightcounter
}
checkForBumperPress=0; //main loop stops checking for a bumper press
}
//robot rotates 90 degrees to the right
void rotateRight(){
drive_goto(26, -25);
}
//robot turns 90 degrees to the left
void turnLeft(int iter){
if(iter == 0)
drive_goto(-5, -5);
drive_goto(-26, 25);
}
//robot keeps driving till the bumper is pressed
void startup(){
while(input(bumperPort)==1){
drive_speed(40,40);
}
turnLeft(0);
}