Skip to content

Carbot Leaves The Arena

January 26, 2015

One last floor show yesterday afternoon with results comparable to the previous post. It’s not super-fast but i’m less concerned about that than the imprecise turns.

I’m going to retire this for now and get back to it later. For this trial I had crudely stabilized the tower that mounts the sharp GPD212 sensor and tweaked the code(version O-carbotS1.c) somewhat. I’m going to document it here and put the source under version control. To get to the bottom of the wonky turns I would have to put a transmitter on it. The original carbot used a pair of xbee’s for remote programming and telemetry. I could revive them or get my bluetooth module working but either one is more digging around than i want to do this week. I may bring the comm. bits to florida with me and bench test them.
15-01-26 buttress

For the record:

  • Source code: O-carbotS1.c
  • Toggle table software pwm softpwm.h/.c
  • Sharp GP2D12 IR sensor facing forward with turn triggered at a reading of 160.
  • MCP3002 Analog to Digital Converter chip over SPI
  • Sharpy() routine embedded in source
  • Full power turns in 60 ms increments
  • Seeed Studio Ultrasonic Sensor V1.0 facing right with target distance of 30 cm
  • Ping routine pingQ in pingbn.h/.c
  • Base cruising speed of 192/255 with 32 on one side for course correction
  • Takes about 8-10¬†seconds to circle the 5′ by 5′ arena.
//olduino carbot simple version Jan 24, 2015
//
#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 160 //max front barrier proximity
#define minwdist 30 //min right barrier dist
#define basespeed 192
#define lowspeed 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
}
void killdrive(){//stops both motors
	digitalWrite(pwmb,LOW);//kill power right
	digitalWrite(pwma,LOW);//kill power left
}
void hardleft(){//60 ms turn with left wheel running backwards
	printf("L>\n");
	digitalWrite(pwmb,HIGH);//full power right
	digitalWrite(pwma,HIGH);//full power left
	digitalWrite(bin1,HIGH); digitalWrite(bin2,LOW);
	digitalWrite(ain1,LOW); digitalWrite(ain2,HIGH); //reverse left wheel
	delay(60);
}

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 ");
    }
    else{
		analogWrite(pwma,lowspeed);analogWrite(pwmb,basespeed); //bear left
		printf(" wvlft ");
    }
}
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 ");
    }
    else{
		analogWrite(pwma,basespeed);analogWrite(pwmb,lowspeed); //bear right
		printf(" wvrt ");
    }
}
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{
		toofar();
	}
	//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(){
	unsigned int ttl=300; //time to live limit,
	printf("Simpler Panopticon Carbot O-carbotS1.c\n");
	PIN4=0x80;out(4,0x80);
	while(ttl!=00){
		printf("@ %d:",ttl);
		digitalWrite(7,LOW);fprox=sharpy(); digitalWrite(7,HIGH);
		printf("<%d ",fprox);
		if (fprox<maxfprox){ // more than 1 ft from barrier
			killdrive();//kill any turn that was going on
			printf("F>\n");
			cruiseAlongWall();//set a course to track the wall
		}else{
			hardleft();
		}
		ttl--; //reduce time to live
	}
	PIN4=0;out(4,0); //kill it all
}
#include <olduino.c>
#include <nstdlib.c>
#include "softpwm.c"
#include "pingBN.c"
//carbot.h defines for the carbot wall follower
#define pwma 1	//pwm for $a side
#define pwmb 0	//pwm for $b side
#define bin1 3	//front motor pole for $b
#define bin2 2	//back
#define ain1 4	//front motor pole for $a side
#define ain2 5	//back
//softpwm.h defines for olduino software pulse width management.
unsigned int step=0;
unsigned char toggle[8]={0,0,0,0,0,0,0,0}; //toggle table for software pwm
unsigned char dct[2]={0,0}; //duty cycle table for pwm bits
void analogWrite(unsigned int bit, unsigned int val);
void pwmstep();	//run one step of the software pwm cycle
void pwmcycleN(unsigned int howmany); //run howmany*8 steps of the software pwm
//softpwm.c routines for olduino software pulse width management.
//Oct 12 - pwmstep included in pwmcycleN
void analogWrite(unsigned int bit, unsigned int val){
	//printf("analogWrite(%d,%d)\n",bit,val);
	if (bit<2){
		dct[bit]=val>>5; //step down range to 0..7
		//printf("dct[%d]=%cx\n",bit,dct[bit]);
	}
}
void pwmcycleN(unsigned int howmany){ //run howmany*8 steps of the software pwm
	unsigned int step; //toggle table index
	unsigned char * togglep=toggle; //maybe togglep will be in a register
	while(howmany-->0){
		togglep[dct[0]]|=1; //set the end of the duty cycle for bit 0
		togglep[dct[1]]|=2; //set the end of the duty cycle for bit 1
		togglep[0]^=~PIN4; //set up with current state of port4 low bits
		togglep[0]&=0x03; //only the pwm bits are set
		for(step=0;step<8;step++){
			PIN4^=togglep[step];	//toggles the bit off if the duty cycle is over
			out(4,PIN4);
			//printf("dct[0]=%cx, step %d, toggle[%d]=%cx, PIN4=%cx\n",dct[0],step, step, toggle[step],PIN4);
			togglep[step]=0;
		}
	}
}
unsigned int	pingQ(){ //read an ultrasonic ping distance sensor
//pulsing the Q line high then low starts the ultrasonic sensor
//it responds in around a ms with a pulse on EF1 with a length equal to the echo time
//the distance in cm is approximately (flight time in microseconds) /29 (inverse speed of sound in us/cm)/2 (round trip)
//with the mc running at 1.6mhz, each instruction is about 10 us
// so a 3 inst. loop yields a rough result in cm for the round trip
//with the mc running at 4mhz each instruction is about 4 us and you need a 7 instruction loop
	asm("	req\n	req\n"	//make sure Q is low
		"	seq\n"	//begin a short pulse
		"	seq\n	seq\n	seq\n	seq\n"	//extend the pulse to 20 us
		"	req\n" //end the short pulse
		"	ldaD R15,0\n"
		"	lbr yn1\n"	//branch to aligned code area
		"	align 8\n" //align for short branches
		"yn1: b1 yn1\n"	//wait for the return pulse to start
		"nn1: inc R15\n"	//count pulse length
		"	sex sp\n"	//soak up one instruction time
	);
#ifdef __CPUSPEED4 //need to add 4 more instruction times for faster cpu
	asm("	sex sp\n	sex sp\n	sex sp\n	sex sp\n");
#endif
	asm("	bn1 nn1\n"	//keep counting while return pulse is high
		"	Cretn\n"		//return to caller with count in r15
	);
//the cretn above returns the count to the caller.
	return 0; //not executed - just prevents an error message
}
Advertisements

From → Uncategorized

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: