[markgryzwa]

Here is some servo code that I put together to try to run some basic robot functions. I've not tested in out for polarity yet but the ideas are correct.

#include <Servo.h>

// global variables

// ------------- Servo setup ------------------------------
Servo myservo_left;
Servo myservo_right;

int left_pwm = 10; // set up the servo connections for the Jaguar controllers
int right_pwm = 9;
int rate = 0;


// ------- Spike relay control signals -------------------
int FWD = 1;
int REV = 2;
int OFF = 0;
int spike_white = 0; // pin 0 for spike signal wire
int spike_red = 1; // pin 1 for spike red wire


void setup() {
// put your setup code here, to run once:

myservo_left.attach(left_pwm);
myservo_right.attach(right_pwm);

pinMode(spike_white, OUTPUT);
pinMode(spike_red, OUTPUT);

}

void loop() {

// Set up the default
Spike(OFF);


// Make stuff happen
Forward(100, 1000); // move forward as a % of full speed for ms Foward(%*full speed,ms)
delay(1000);

Spike(FWD);
Reverse(100, 1000);
delay(1000);

Spike(REV);
Right(50, 100);
delay(1000);

Left(50,200);
delay(1000);

Spike(OFF);
Right(50, 100);
delay(6000);


}



//------------------ Subroutines ---------------------------

//-----------------------------------------------------------
void Forward(int rate, int duration) {

int spd = rate * 500; // set speed as a % of rate

myservo_left.writeMicroseconds(1500+spd);
myservo_right.writeMicroseconds(1500-spd);
delay(duration);
myservo_left.writeMicroseconds(1500);
myservo_right.writeMicroseconds(1500);
}

//-----------------------------------------------------------
void Reverse(int rate, int duration) {

int spd = rate * 500; // set speed as a % of rate

myservo_left.writeMicroseconds(1500-spd);
myservo_right.writeMicroseconds(1500+spd);
delay(duration);
myservo_left.writeMicroseconds(1500);
myservo_right.writeMicroseconds(1500);

}

//------------------------------------------------------------
void Left(int rate, int duration) {

int spd = rate * 500; // set speed as a % of rate

myservo_left.writeMicroseconds(1500+spd);
myservo_right.writeMicroseconds(1500+spd);
delay(duration);
myservo_left.writeMicroseconds(1500);
myservo_right.writeMicroseconds(1500);

}


//------------------------------------------------------------
void Right(int rate, int duration) {

int spd = rate * 500; // set speed as a % of rate

myservo_left.writeMicroseconds(1500-spd);
myservo_right.writeMicroseconds(1500-spd);
delay(duration);
myservo_left.writeMicroseconds(1500);
myservo_right.writeMicroseconds(1500);

}



//---------------------------------------------------------------
void Spike(int state) { //states can be 0=Off, 1 = FWD, 2=REV

switch (state) {
case 0:
digitalWrite(spike_red, LOW);
digitalWrite(spike_white, LOW);
break;

case 1:
digitalWrite(spike_red, HIGH);
digitalWrite(spike_white, LOW);
break;

case 2:
digitalWrite(spike_red, LOW);
digitalWrite(spike_white, HIGH);
break;

default:
digitalWrite(spike_red, LOW);
digitalWrite(spike_white, LOW);
break;
}

}