mandag 23. februar 2009

The Code for ArduTnak

First of all the code is partly in norwegian an partly in english, but i think you shud be able to understand most of the code.

It use pwm to controll the speed of the tank, and use four pins for direction, two for each motor.

The function "kjor" is the main driving function. It has a the folowing syntax:

void kjor(time in mils, direction, speed Right is by default 255, speed Right is by default 255

The other driving functon is a turn functon where both motors run at full speed in oposite directons.

void snu( decres to turn )

//negative turns it counter clockwhise and positive turn it clockwhise

I'am not completly sure the code whil compile, becuse i dont have the arduino on this computer.

Anyway here is the code:


//Motor pin
#define H1 3
#define H2 2
#define HE 6
#define V1 19
#define V2 18
#define VE 11

#define servoPin 9

#define motorFaktor 15 //Kor mykje ein m� gonge gradene for � f� millisec for gradene
#define avgFactor 3
#define turnDist 20 //How far far from opsticle when turn

byte dist [3]; //|left dist | middle dist | right dist
byte distLong; //Longest distance reading
byte distDir; //Direction of longest distance 0 = left, 1 = middle, 2 = right

Servo distServo;

void kjor(int tid, byte retning, byte fartH = 255, byte fartV = 255);//Kjore fungsjon 1 er fram og 0 er bakover
void snu( int grader); //
void motorAv();
int distRead();

void setup()
pinMode( H1, OUTPUT);
pinMode( H2, OUTPUT);
pinMode( HE, OUTPUT);
pinMode( V1, OUTPUT);
pinMode( V2, OUTPUT);
pinMode( VE, OUTPUT);
snu( -30);

//Servo motor

//Serial config
Serial.begin( 9600);



void loop()
//Read left
dist [0] = distRead();

//Read Middle
dist[1] = distRead();

//Read right
dist[2] = distRead();

//Find biggest
distLong = dist[0];
for( byte x = 0; x < distlong =" dist[x];" distdir =" x;" distdir ="=" distdir ="=" distdir ="=" retning ="=" retning ="="> 0)
digitalWrite( HE, HIGH);
digitalWrite( VE, HIGH);
digitalWrite( H1, HIGH);
digitalWrite( H2, LOW);
digitalWrite( V1, LOW);
digitalWrite( V2, HIGH);

delay( grader * motorFaktor);


else if (grader > 0)
digitalWrite( HE, HIGH);
digitalWrite( VE, HIGH);
digitalWrite( H1, LOW);
digitalWrite( H2, HIGH);
digitalWrite( V1, HIGH);
digitalWrite( V2, LOW);

delay((grader - grader - grader) * motorFaktor);



void motorAv()
digitalWrite( H1, LOW);
digitalWrite( H2, LOW);
digitalWrite( V1, LOW);
digitalWrite( V2, LOW);

digitalWrite( HE, LOW);
digitalWrite( VE, LOW);

int distRead()
float x,y; // Distance and Analog voltage output
int avg = 0;
for ( byte i=0; i < avgFactor; i++ )
y = analogRead(0); // Read volt out if the sensor connect to channel 0
y = y/204.8; // Convert BCD to DEC by multiply voltage by 1024/5
if (y < 0.5)
y = 0.5;
if ( y > 2.5)
y = 2.5;
if ( (y >= 0.5) && (y <= 2.8) ) {
x = (y-0.19)/20.99; // Solve the linear equation
x = 1/x; // Inverse back get distance in cm.

avg =+ x;


return avg/avgFactor;

Ingen kommentarer:

Legg inn en kommentar