• View
• Edit
• History
• Print

MotorShield

//-----------
// This code is set up to be modified for mutliples motors and more configurable. Unused code is commented out.
// using Adafruit Motor shield library
// original copyright Adafruit Industries LLC, 2009 modified by Louis Katz
// & copyright Louis Katz 2012
// this code is public domain, enjoy!
// This version reads sharp IR Distance Sensors, and a photoresistor on a servo in a subroutine
//This version uses separate functions for several motor routines forward , backward, spin right spin left and soft turns
//This verions has 4 separate motor controlsin a subroutine
#include <AFMotor.h>// This allows functions in the library AFMotor to be used in the program
#include <Servo.h> 
AF_DCMotor motorLF(4);//LeftFront This assigns names to the motorshield stepper outputs and configures them as DC motors.
AF_DCMotor motorRF(3);//RightFront
AF_DCMotor motorLR(1);//LeftRear
AF_DCMotor motorRR(2);//RightRear
Servo servo1;

/* this would be a simpler program if I used one output for the left two motors and another for the right,
but I wanted to be able to control all four independently*/
//------------------------------
//Global Configuration Variables. Global variables can be used inside funtions including setup and loop. If you change them in one place they are changed in all.
int maxspeed = 255;// Sets maximum voltage to the DC motors. At 255 this is supposedly 1.5 V lower than the motor shield supply voltage.
int slowspeed = 100;
//int accel=1; //acceleration 1-255". 1 is most gradual. 255 is on/off. Note: "1" saves wear and tear on the motor and gearbox. 
//int decel=1;//deceleration 1-255. "1"is most gradual 255 is on off. Note: "1" will prevent quick stops.
//int continuous = 1; //"0" turns off motor (.release) before each sensor read. "1" leaves it going unless told to stop.
//Grove IR Distance Sensor Sharp 2Y0A2
const int analogInPin0 = A3;  // Analog input pin that Right Distance Sensor A0,A1. Const -constant, cant be changed.
const int analogInPin1 = A5; // Left Distance Sensor A2,A3
const int analogInPin2 = A0;// light sensor Forward
const int analogInPin3 = A1;// light sensor On servo

 // Analog input pin that  is attached to
int sensorValue0 = 0;        // value read from the Right
int sensorValue1 = 0;        // value read from the Left
int sensorValue2 = 0;        // value read from Forward LightSensor
int lightLeft = 0; int lightCenter = 0; int lightRight = 0;//moving light sensor readings

//int sensorValues[] = {0,0,0,0};  unused

//--------------------------------
int Runtime = 250; 
 /* int LFRunSpeed=;int LFDirection;int RFRunSpeed;int RFDirection;int LRRunSpeed;int LRDirection;
  int RRRunSpeed;
  int RRDirection;*/
  int Delay;
//--------------------------------
void setup() {


  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");
  servo1.attach(9); // turn on servo
  // Turnoff motor
  motorLF.run(RELEASE);
  motorRF.run(RELEASE);
  motorLR.run(RELEASE);
  motorRR.run(RELEASE);
}

void loop() {
  uint8_t i;



  int x; //place holder for function return
  int  sum0 =0;//Right Sensor Sum for Averaging  
  int  sum1 =0;//Left
  runForward();
  delay (Runtime); //rest for debuging what decisions its making
  runBackward();
  delay (Runtime);
  spinLeft();
  delay (Runtime);
  spinRight();
  delay (Runtime);
  delay (Runtime);
  softLeft();
  delay (Runtime*5);
  softRight();
  delay (Runtime*5);

  //x= checkSensors(0,1,2,3,4);
 /* x=checkSensors (x);
  Serial.print("RightDist  = " );                       
  Serial.print(sensorValue0);
  Serial.print("   ||  LeftDist  = " );                       
  Serial.print(sensorValue1); 
    Serial.print("|| Right Darkness = ");
  Serial.print(sensorValue2);
  Serial.print(" || Left Darkness = ");
  Serial.println(lightLeft);
  Serial.println(lightCenter);
  Serial.println(lightRight);
*/
}// end of main program
//Functions Are Defined Below
//runForward Function code
void runForward ()
{motorLF.run(FORWARD); motorRF.run(FORWARD);motorRR.run(FORWARD);motorLR.run(FORWARD);
 motorLF.setSpeed(maxspeed); motorRF.setSpeed(maxspeed);motorLR.setSpeed(maxspeed); motorRR.setSpeed(maxspeed);
   delay (Runtime*2);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
void runBackward ()
{motorLF.run(BACKWARD); motorRF.run(BACKWARD);motorRR.run(BACKWARD);motorLR.run(BACKWARD);
 motorLF.setSpeed(maxspeed); motorRF.setSpeed(maxspeed);motorLR.setSpeed(maxspeed); motorRR.setSpeed(maxspeed);
   delay (Runtime*2);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
void spinLeft ()
{motorLF.run(BACKWARD); motorRF.run(FORWARD);motorRR.run(FORWARD);motorLR.run(BACKWARD);
 motorLF.setSpeed(maxspeed); motorRF.setSpeed(maxspeed);motorLR.setSpeed(maxspeed); motorRR.setSpeed(maxspeed);
   delay (Runtime *3);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
void spinRight ()
{motorLF.run(FORWARD); motorRF.run(BACKWARD);motorRR.run(BACKWARD);motorLR.run(FORWARD);
 motorLF.setSpeed(maxspeed); motorRF.setSpeed(maxspeed);motorLR.setSpeed(maxspeed); motorRR.setSpeed(maxspeed);
   delay (Runtime*3);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
void softRight ()
{
 motorLF.run(FORWARD); motorRF.run(FORWARD);motorRR.run(FORWARD);motorLR.run(FORWARD);
 motorLF.setSpeed(maxspeed); motorRF.setSpeed(slowspeed);motorLR.setSpeed(maxspeed); motorRR.setSpeed(slowspeed);
   delay (Runtime*5);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
void softLeft ()
{
 motorLF.run(FORWARD); motorRF.run(FORWARD);motorRR.run(FORWARD);motorLR.run(FORWARD);
 motorLF.setSpeed(slowspeed); motorRF.setSpeed(maxspeed);motorLR.setSpeed(slowspeed); motorRR.setSpeed(maxspeed);
   delay (Runtime*5);
  motorLF.run(RELEASE); motorRF.run(RELEASE); motorLR.run(RELEASE); motorRR.run(RELEASE); 
}
//checkSensors function code
//int checkSensors (int sensorValue0, int sensorValue1, int sensorValue2, int sensorValue3, int sensorValue4)
int checkSensors(int x)
//distance sensor
{ 
  int i;//counter
  //int x; //place holder for function return
  int  sum0 =0;//Right Sensor Sum for Averaging  
  int  sum1 =0;//Left
  // read the analog in value:
  for (int i =0;i<20;i++)
  {

    sensorValue0 = analogRead(analogInPin0); 
    sum0 =sum0 +sensorValue0;
    sensorValue1 = analogRead(analogInPin1); 
    sum1 =sum1 +sensorValue1;
    }
  sensorValue0= sum0 / 20 ; 
  sensorValue1= sum1 / 20 ; 
  sensorValue2 = analogRead(analogInPin2);
 //Light Sensor Check

  servo1.write(0);
  delay (1000);
  lightLeft = analogRead(analogInPin3);
  delay (10);
  servo1.write(90);
  delay (1000);
  lightCenter = analogRead(analogInPin3);
  delay (10);
  servo1.write(150);
  delay (1000);
  lightRight = analogRead(analogInPin3);
  delay (10);

  Serial.println(lightLeft);//.debug
  Serial.println(lightCenter);//.debug
  Serial.println(lightRight);//.debug
  //int sensorValues[] = {sensorValue0,sensorValue1,sensorValue2,sensorValue3};


  // print the results to the serial monitor:
 /* Serial.print("RightDist  = " );                       
  Serial.print(sensorValue0);
  Serial.print("   ||  LeftDist  = " );                       
  Serial.print(sensorValue1); 
    Serial.print("|| Right Darkness = ");
  Serial.print(sensorValue2);
  Serial.print(" || Left Darkness = ");
  Serial.println(sensorValue3);
  // wait 500 milliseconds before the next loop*/
  delay(500); 
  return x ;
  //sensorValues[sensorValue0,sensorValue1,sensorValue2,sensorValue3] ;


}
//-----------