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();
}

Add a Comment

Your email address will not be published. Required fields are marked *