Test program Drive and scan
// Forward-turn Sweep and tilt with functions.Dagu mini driver
// David Baxter November 10th 2014
#include <Servo.h>
Servo myservo_tilt, myservo_pan; // create servo object to control a servo
int pos = 0; // variable to store the servo position
const int LEFT_MOTOR_DIR_PIN = 7;
const int LEFT_MOTOR_PWM_PIN = 9;
const int RIGHT_MOTOR_DIR_PIN = 8;
const int RIGHT_MOTOR_PWM_PIN = 10;
const int DRIVE_FORWARD_TIME_MS = 1500;
const int TURN_TIME_MS = 300;
void setup()
{
myservo_tilt.attach(6); // attaches the servo on pin D6 to the tilt servo object
myservo_pan.attach(11); // attaches the servo on pin D11 to the pan servo object
// Setup the drive motor pins
pinMode( LEFT_MOTOR_DIR_PIN, OUTPUT );
pinMode( LEFT_MOTOR_PWM_PIN, OUTPUT );
pinMode( RIGHT_MOTOR_DIR_PIN, OUTPUT );
pinMode( RIGHT_MOTOR_PWM_PIN, OUTPUT );
}
//———————————————
//returntype functionNAME( arguments ){
void tiltUPDOWN(){
for(pos = 90; pos < 180; pos += 1) // tilts up from 90-180 degrees
{ // in steps of 1 degree
myservo_tilt.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
for(pos = 180; pos>=90; pos-=1) // tilts down from 180 degrees to 90 degrees
{
myservo_tilt.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
} // return returntype; is implied by closing curly bracket.
void panFORTH(){
for(pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees PANS ROUND
{ // in steps of 1 degree
myservo_pan.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
} // return returntype; is implied by closing curly bracket.
void panBACK(){
for(pos = 180; pos>=0; pos-=1) // goes from 180 degrees to 0 degrees PANS BACK ROUND
{
myservo_pan.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
}
//—————————————————-
void loop()
{
// Drive Forwards at 100%
digitalWrite( LEFT_MOTOR_DIR_PIN, LOW );
digitalWrite( RIGHT_MOTOR_DIR_PIN, LOW );
analogWrite( LEFT_MOTOR_PWM_PIN, 255 );
analogWrite( RIGHT_MOTOR_PWM_PIN, 255 );
delay( DRIVE_FORWARD_TIME_MS );
//Turn left atv 50%
digitalWrite( LEFT_MOTOR_DIR_PIN, LOW );
digitalWrite( RIGHT_MOTOR_DIR_PIN, HIGH );
analogWrite( LEFT_MOTOR_PWM_PIN, 64 );
analogWrite( RIGHT_MOTOR_PWM_PIN, 64 );
delay( TURN_TIME_MS );
tiltUPDOWN();
panFORTH();
tiltUPDOWN();
panBACK();
}