Hudson
Jump to navigation
Jump to search
Hudson Information and Code
=Hudson Robot with Interrupt MIGHT work
Hudson, I have not had time to try this...
#include <YD_MotorDriver1.h> // For control of the two DC Motors
#include <NewPing.h> // Runs the Ultrasonic Distance Sensor
/*-----( Declare Constants and Pin Numbers )-----*/
// NOTE: Pins 9 (Motor1) and 10 (Motor2) are predefined, unchangeable
#define Motor1A 8
#define Motor1B 7
#define Motor2C 6
#define Motor2D 5
#define RampDelay 10
#define StartMoveSpeed 200 // Motor Driver value for start of motion
#define SlowMoveSpeed 280
#define SlowMoveAdjust 0 // Adjust for straight move: - Left + Right??
#define MediumMoveSpeed 300
#define MediumMoveAdjust 0 // Adjust for straight move: - Left + Right
#define FastMoveSpeed 350
#define FastMoveAdjust 0 // Adjust for straight move: - Left + Right
// Ultrasonic Sensor Pins
#define TRIGGER_PIN 2 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define SWITCH_INTERRUPT_PIN 3
#define ECHO_PIN 4 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
// Servo
#define SERVO_PIN 11 // The "Look Around" servo
#define PIN13_LED 13 // The onboard LED: Lights when an object is sensed by the Ultrasonic
// Add a Beeper on pin 13 if you want
//----( "TARGET FOUND" DIRECTIONS (index into TargetArray )---------
#define TARGET_FOUND_ANY 0 // Values will be "true" or "false"
#define TARGET_LEFT 1
#define TARGET_LEFT_CENTER 2
#define TARGET_CENTER 3
#define TARGET_RIGHT_CENTER 4
#define TARGET_RIGHT 5
#define TARGET_ARRAY_SIZE 6
#define TARGET_TOO_CLOSE 25 // cm
/*-----( Declare objects )-----*/
YD_MotorDriver1 RobotDriver(Motor1A, Motor1B, Motor2C, Motor2D); // Set pins
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Set pins and maximum distance.
/*-----( Declare Variables )-----*/
unsigned int uS; // Result of a ping: MicroSeconds
unsigned int cm; // Distance calculated for ping (0 = outside set distance range)
unsigned int cm_now; // For test
int TargetArray[TARGET_ARRAY_SIZE]; // Holds the directions a Target was found in
int DirectionsToLook = 3; // For LookAround()
int ServoDirectionData[3] = { 2500, 1600, 600};
/*-----( Define the 'States' the robot can be in )-----*/
volatile byte RobotState; // Make this changeable by the Interrupt Service Routine
#define START_STATE 1
#define STOP_LOOK_STATE 2
#define TARGET_CENTER_STATE 3
#define TARGET_LEFT_STATE 4
#define TARGET_RIGHT_STATE 5
void setup ()
{
Serial.begin(115200);
delay(1000);
Serial.println("YourDuino Robot Kit Test");
//--NOTE: Ultrasonic Sensor and Motor Pins set to OUTPUT by their libraries
pinMode(SWITCH_INTERRUPT_PIN, INPUT);
pinMode(SERVO_PIN, OUTPUT);
pinMode(PIN13_LED, OUTPUT);
attachInterrupt(digitalPinToInterrupt(SWITCH_INTERRUPT_PIN), Crash, FALLING); // HUDSON
RobotDriver.init();
RobotState = START_STATE;
}
void loop() {
/*-----( Control the Robot by changing the "State" it is in. )-----*/
switch (RobotState)
{
case START_STATE: // Start here, return here if no obstacles
Serial.println("+++ RoboStartState");
digitalWrite(PIN13_LED, LOW); // LED Means Target Too Close. Turn it off for now.
ForwardFast(); // Get a good start moving forward
delay(1000);
ForwardFast(); // Continue moving forward
delay(1250);
RobotState = STOP_LOOK_STATE ; // Now change to state to look for obstacles
break;
case STOP_LOOK_STATE: // Look around for obstacles; take different actions if found
Serial.println("+++ RoboStopLookState");
Stop(); // Stop moving forward
for (int i = 0; i < TARGET_ARRAY_SIZE; i++) TargetArray[i] = false;
LookAround(); // Ping Ultrasonic in different directions, Set TargetArray
if (TargetArray[TARGET_CENTER ] == true) RobotState = TARGET_CENTER_STATE;
else if (TargetArray[TARGET_LEFT ] == true) RobotState = TARGET_LEFT_STATE;
else if (TargetArray[TARGET_RIGHT ] == true) RobotState = TARGET_RIGHT_STATE;
else RobotState = START_STATE;
break;
case TARGET_CENTER_STATE: // Obstacle straight ahead; back up then turn around
Serial.println("***** RoboTargetCenter");
Stop();
BackwardSlow(500);
SpinLeft(1100); // try for 180 degrees turn
delay(500);
RobotState = START_STATE;
break;
case TARGET_LEFT_STATE: // Obstacle on left; back up then turn right
Serial.println("***** RoboTargetLeft");
Stop();
BackwardSlow(500);
SpinRight(500);
delay(500);
RobotState = START_STATE;
break;
case TARGET_RIGHT_STATE: // Obstacle on right; back up then turn left
Serial.println("***** RoboTargetRight");
Stop();
delay(500);
BackwardSlow(500);
SpinLeft(500);
delay(500);
RobotState = START_STATE;
break;
} // end of switch
} // end of loop
/*----------------( Declare User-written Functions )---------------*/
void Crash() // Interrupt Service Routine for MicroSwitch crash sensor
{
RobotState = TARGET_CENTER_STATE; // Make the robot stop immediately and turn around..
}
//------( MOTOR CONTROL FUNCTIONS )----------------
void ForwardSlow()
{
RobotDriver.Motor1Speed(SlowMoveSpeed + SlowMoveAdjust);
RobotDriver.Motor2Speed(SlowMoveSpeed - SlowMoveAdjust);
}
/*---------------------------*/
void ForwardMedium()
{
RobotDriver.Motor1Speed(MediumMoveSpeed + MediumMoveAdjust);
RobotDriver.Motor2Speed(MediumMoveSpeed - MediumMoveAdjust);
}
/*---------------------------*/
void ForwardFast()
{
RobotDriver.Motor1Speed(FastMoveSpeed + FastMoveAdjust);
RobotDriver.Motor2Speed(FastMoveSpeed - FastMoveAdjust);
}
/*---------------------------*/
void BackwardSlow(int HowMuch)
{
RobotDriver.Motor1Speed(- SlowMoveSpeed );
RobotDriver.Motor2Speed(- SlowMoveSpeed );
delay(HowMuch);
Stop();
}
/*---------------------------*/
void BackwardMedium(int HowMuch)
{
RobotDriver.Motor1Speed(- MediumMoveSpeed);
RobotDriver.Motor2Speed(- MediumMoveSpeed);
delay(HowMuch);
Stop();
}
/*---------------------------*/
void BackwardFast(int HowMuch)
{
RobotDriver.Motor1Speed(- FastMoveSpeed);
RobotDriver.Motor2Speed(- FastMoveSpeed);
delay(HowMuch);
Stop();
}
/*---------------------------*/
void Stop()
{
RobotDriver.Motor1Speed(0);
RobotDriver.Motor2Speed(0);
}
/*---------------------------*/
void SpinLeft(int HowMuch)
{
RobotDriver.Motor1Speed( MediumMoveSpeed);
RobotDriver.Motor2Speed(- MediumMoveSpeed);
delay(HowMuch);
Stop();
}
/*---------------------------*/
void SpinRight(int HowMuch)
{
RobotDriver.Motor1Speed(- MediumMoveSpeed);
RobotDriver.Motor2Speed( MediumMoveSpeed);
delay(HowMuch);
Stop();
}
/*---------------------------*/
unsigned int PingBlink()
{
uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
cm = uS / US_ROUNDTRIP_CM; // Convert ping time to distance in cm
Serial.print(" cm = ");
Serial.print(cm, DEC);
if ((cm < 40) && (cm != 0))
{
return (cm);
}
else
{
return (100); // No Valid Distance
}
}// end PingBlink
/*---------------------------*/
void PointServo(int ServoAngle)
{
for (int i = 0; i < 20; i++) // Send the pulse 10 times
{
digitalWrite(SERVO_PIN, HIGH);
delayMicroseconds(ServoAngle);
digitalWrite(SERVO_PIN, LOW);
delay(20);
}
}// PointServo end
/*---------------------------*/
void LookAround() // Sets next state if Target Found
{
for (int Direction = 0; Direction < DirectionsToLook ; Direction ++)
{
Serial.print("DIRECTION = ");
Serial.print(Direction, DEC);
PointServo(ServoDirectionData[Direction]); // Get servo pulse width from array
delay(200);
cm_now = PingBlink(); // Read the Ultrasonic distance
Serial.print(" cm_now = ");
Serial.println(cm_now, DEC);
if (cm_now < TARGET_TOO_CLOSE) digitalWrite(PIN13_LED, HIGH);
if (cm_now < 40)
{
TargetArray[TARGET_FOUND_ANY ] = true;
}
else TargetArray[TARGET_FOUND_ANY ] = false;
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 2)) //LEFT
{
TargetArray[TARGET_LEFT ] = true;
Serial.println("TargetLeft");
}
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 1)) //Center
{
TargetArray[TARGET_CENTER ] = true;
Serial.println("TargetCenter");
}
if ((cm_now < TARGET_TOO_CLOSE) && (Direction == 0)) //RIGHT
{
TargetArray[TARGET_RIGHT ] = true;
Serial.println("TargetRight");
}
}// END Directions
}// END LookAround
/*--- THE END )---*/
HudsonSketchExample1202
/* HUDSON LUSK SKETCH EXAMPLE 12/02/2020
Read microswitch and control LED.
terry@yourduino.com */
/*-----( Import needed libraries )-----*/
//None
/*-----( Declare Constants and Pin Numbers )-----*/
#define LED_PIN 13 // Or change to what you want
#define SWITCH_PIN 2 // NOTE: a convention (Not a requirement) is to have Constants names all uppercase.
/*-----( Declare objects )-----*/
//None
/*-----( Declare Variables )-----*/
int SwitchValue;
void setup() /****** SETUP: RUNS ONCE ******/
{
pinMode(LED_PIN, OUTPUT);
pinMode(SWITCH_PIN, INPUT); // Not actually necessary for the red PC Board Microswitch. Why?
// Sometimes , if you are connecting a plain switch from Arduino pin to Ground, you can use:
// pinMode(SWITCH_PIN, INPUT_PULLUP); // That makes Arduino internally connect a resistor from the pin to +5V. Why?
}//--(end setup )---
void loop() /****** LOOP: RUNS CONSTANTLY ******/
{
SwitchValue = digitalRead(SWITCH_PIN);
digitalWrite(LED_PIN, ! SwitchValue); // Try removing the "!" (That is a 'not' symbol. Say it as "BANG!" )
}//--(end main loop )---
/*-----( Declare User-written Functions )-----*/
//none
//*********( THE END )***********
HudsonSketchTemplate-SAVE
Copy this and save to start a new sketch
/* HUDSON LUSK SKETCH TEMPLATE
Start with this for most
more-complex Arduino Sketches
Save a version for each project and save this original...
terry@yourduino.com */
/*-----( Import needed libraries )-----*/
/*-----( Declare Constants and Pin Numbers )-----*/
/*-----( Declare objects )-----*/
/*-----( Declare Variables )-----*/
void setup() /****** SETUP: RUNS ONCE ******/
{
}//--(end setup )---
void loop() /****** LOOP: RUNS CONSTANTLY ******/
{
}//--(end main loop )---
/*-----( Declare User-written Functions )-----*/
/None
//*********( THE END )***********