Skip to content

Carbot Races Like He Means It!

August 17, 2015

After the victory lap yesterday I made a few tweaks to the code, speeding up the straight line speed, changing the turn to stop the inside wheel rather than actually reversing it, increasing the distance at which i initiate the turn, and monitoring the front sensor more closely to stop the turn once I’m clear.

The result seems pretty good to me. I suspect I could get better speed on the straightaway by running the motor full out instead of using the software pwm but I’m happy with this. If I ever find someone to race, I’ll tune it for the specific situation.

//olduino carbot Simple version 2 August 16
//15-04-26 using <> version of cpu1802spd4port7.h
//15-04-29 cruising speed increased, min/max wall distance
//left turn now 5 ms, deliberate braking at end of turn
// not too successful in wall following.
// Aug 17 turn sequence now loops in place, looking for clearance of 150
//this was not good - just consistently turned too much
#include <olduino.h>
#include <nstdlib.h> //supports c d s x l cx
#include <cpu1802spd4port7.h>
#include "softpwm.h"
#include "carbot.h"
#include "pingBN.h"
unsigned int wdist,oldwdist, fprox; // distance to wall on right current & previous, proximity of barrier in front
#define maxfprox 100 //max front barrier proximity
#define maxwdist 32 //min right barrier dist
#define minwdist 28 //min right barrier dist
#define basespeed 255	//was 192
#define lowspeed 32	//was 32
unsigned int sharpy(){  //reads the Sharp GP2D12 IR rangefinder via the MCP3002 ADC, returns raw ADC value
	asm("	dec	sp\n" //make a work area
		"	sex 3\n"	//set x to pc
		"	out 6\n	db 0x68\n"	//send 68 to read channel 0
		"	sex sp\n	inp 6\n" //pick up top bits
		"	ani 3\n	phi 15\n"	//put bits 8&9 of result into return register

		"	sex 3\n"	//set x to pc
		"	out 6\n	db 0xAA\n"	//send pattern to read low bits
		"	sex sp\n	inp 6\n	plo 15\n" //pick up low bits to return register
		"	inc	sp\n"	//restore stack pointer
		"	cretn\n");	//actual return
	return 0;	//dummy return for compiler
}
unsigned int sharp2(){//read the sensor twice and return the lower reading to ignore spikes
	unsigned int fprox1,fprox2;
	digitalWrite(7,LOW);fprox1=sharpy(); digitalWrite(7,HIGH);
	digitalWrite(7,LOW);fprox2=sharpy(); digitalWrite(7,HIGH);
	return min(fprox1,fprox2);
}
void killturn(){//briefly reverses power to both motors
	digitalWrite(pwmb,LOW);//kill power right
	digitalWrite(pwma,LOW);//kill power left
	digitalWrite(ain1,HIGH); digitalWrite(ain2,LOW);
	digitalWrite(bin1,LOW); digitalWrite(bin2,HIGH);
	digitalWrite(pwmb,HIGH);//full power right
	digitalWrite(pwma,LOW);//no power left
	delay(5);
	digitalWrite(pwmb,LOW);//kill power right
	digitalWrite(pwma,LOW);//kill power left
}
void hardleft(){//5 ms turn with left wheel running backwards
	printf("L>\n");
	digitalWrite(pwmb,HIGH);//full power right
	digitalWrite(pwma,LOW);//no power left
	digitalWrite(bin1,HIGH); digitalWrite(bin2,LOW);
	digitalWrite(ain1,LOW); digitalWrite(ain2,HIGH); //reverse left wheel
	delay(5); //nominal delay only
}

void tooclose(){ //if we are closer than minwdist to the wall on our right side
    if (wdist>oldwdist) {//but going the right way
		analogWrite(pwma,basespeed);analogWrite(pwmb,basespeed); //proceed
		printf(" wf-1\n");
    }
    else{
		analogWrite(pwma,lowspeed);analogWrite(pwmb,basespeed); //bear left
		printf(" wvlft\n");
    }
}
void toofar(){//if we are further than minwdist from the wall on our right side
    if (wdist<oldwdist) {//but going the right way
		analogWrite(pwma,basespeed);analogWrite(pwmb,basespeed); //proceed
		printf(" wf-2\n");
    }
    else{
		analogWrite(pwma,basespeed);analogWrite(pwmb,lowspeed); //bear right
		printf(" wvrt\n");
    }
}

void inthezone(){ //if we are between minwdist and maxwdist
	analogWrite(pwma,basespeed);analogWrite(pwmb,basespeed); //proceed
	printf(" wf-3\n");
}

void cruiseAlongWall(){
	if (0!=wdist) oldwdist=wdist; //track the wall distance after the first time
	wdist=pingQ()/2; //get the wall distance in cm
	printf("cw %d\n",wdist);
	if (wdist<minwdist){
		tooclose();
	}else if (wdist>maxwdist){
		toofar();
	}else{
		inthezone();
	}
	//the routines above will have set the motor power levels, now we activate them
	digitalWrite(bin1,HIGH); digitalWrite(bin2,LOW);
	digitalWrite(ain1,HIGH); digitalWrite(ain2,LOW);
	pwmcycleN(15);//forward a bit
}

void main(){
	int ttl=300; //time to live limit,
	//while(1){
	//	cruisealongwall();
		//printf("%d ",pingQ()/2);
		//delay(60);
	//}
	printf("Mode 2 Panopticon Carbot(L=5,B=255,S=32,min/max=28/32,f=100/100) O-carbotS2...\n");
	printf("no reverse on left wheel\n");
	PIN4=0x80;out(4,0x80);
	while(ttl!=00){
		printf("@%d:",ttl);
		fprox=sharp2();
		printf("<%d ",fprox);
		while((fprox<maxfprox)&&ttl>0){
			printf("F>\n");
			cruiseAlongWall();//set a course to track the wall
			ttl--;
			fprox=sharp2();
			printf("%d\n",fprox);
		}
		hardleft(); //begin left turn
		printf("L\n");
		while((fprox>(maxfprox))&&ttl>0){ //until we're clearing the wall
			delay(1);
			ttl--;
			fprox=sharp2();
			printf("%d\n",fprox);
		}
		printf("K\n");
		killturn();
	}
	PIN4=0;out(4,0); //kill it all
}
#include <olduino.c>
#include <nstdlib.c>
#include "softpwm.c"
#include "pingBN.c"
Advertisements

From → carbot

Leave a Comment

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: