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:
#include
//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()
{
//MotorKontroll
pinMode( H1, OUTPUT);
pinMode( H2, OUTPUT);
pinMode( HE, OUTPUT);
pinMode( V1, OUTPUT);
pinMode( V2, OUTPUT);
pinMode( VE, OUTPUT);
snu( -30);
//Servo motor
distServo.attach(servoPin);
distServo.write(90);
//Serial config
Serial.begin( 9600);
delay(1000);
}
void loop()
{
//Read left
distServo.write(60);
delay(300);
dist [0] = distRead();
//Read Middle
distServo.write(90);
delay(300);
dist[1] = distRead();
//Read right
distServo.write(120);
delay(300);
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);
}
motorAv();
}
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;
}
delay(5);
}
return avg/avgFactor;
}
