Here is my full Jarvis sketch to date.
(also thanks to Peter)
Let me know how it looks...still not sure how to incorporate the speedSet() function.
Would I still need to possibly add functions for:
moveForward()
moveBackward()
moveLeft()
moveRight()
moveToNeutral()
Seems like handleThrottAccel() and handleSteerAccel() calculate much of throttle and steering values...
?
// full Jarvis robot sketch
//
//
//
// Update 17Dec12
//
#include <Servo.h>
#include <Stepper.h>
// PING sensors x4:
#define N_PINGS 4
const byte pingPins [N_PINGS] = {22, 23, 24, 25}; // PING pins
const int minRanges [N_PINGS] = {14, 14, 26, 12}; // Thresholds for PING sensors
// Initialize servos
Servo myservoThrottle;
Servo myservoSteering;
Servo myservoSwitch;
// speed values in microseconds
int neutralThrottle = 1500;
int neutralSteering = 1500;
int maxThrotThresh = 1650;
int minThrotThresh = 1350;
int maxSteerThresh = 1650;
int minSteerThresh = 1350;
int actualSpeed = 0; // the speed the motor is currently running at
int targetSpeed = 0; // the speed we want the motor to be running at
unsigned long lastSpeedUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 50; // microseconds
// Stepper motor:
const int stepsPerRevolution = 200; /* change this to fit the number of steps per revolution
for your motor */
// initialize the stepper library on the motor shield
Stepper myStepper(stepsPerRevolution, 12,13);
// give the stepper motor control pins names:
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int dirA = 12;
const int dirB = 13;
int leftStep = 1;
int rightStep = -1;
// PIR motion sensors x2:
// The digital pins connected to the PIR sensors
const int leftPirPin = 26;
const int rightPirPin = 27;
void setup()
{
Serial.begin(9600);
myservoThrottle.attach(53, 1000, 2000); // Attach servos throttle and steering
myservoSteering.attach(52, 1000, 2000);
myservoSwitch.attach(50, 1000, 2000); // Attach servo for aut/man switch
pinMode(leftPirPin, INPUT);
pinMode(rightPirPin, INPUT);
/* Set the PWM and brake pins so that the direction pins
can be used to control the stepper motor: */
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
pinMode(brakeA, OUTPUT);
pinMode(brakeB, OUTPUT);
digitalWrite(pwmA, HIGH);
digitalWrite(pwmB, HIGH);
digitalWrite(brakeA, LOW);
digitalWrite(brakeB, LOW);
// set the motor speed (for multiple steps only):
myStepper.setSpeed(15);
}
void loop()
{
calcPingSensors();
handleThrottAccel();
handleSteerAccel();
//... etc
}
unsigned long calcPingSensors()
{
int range [N_PINGS];
for (int i = 0; i < N_PINGS; ++i) {
range [i] = ping(pingPins [i]);
delay(5);
}
for (int i = 0; i < N_PINGS; ++i) {
Serial.print(range [i]);
Serial.print(" ");
}
Serial.println("inches");
//delay(1000);
}
long ping(int pin)
{
pinMode(pin, OUTPUT);
digitalWrite(pin, LOW);
delayMicroseconds(2);
digitalWrite(pin, HIGH);
delayMicroseconds(5);
digitalWrite(pin, LOW);
pinMode(pin, INPUT);
return (microsecondsToInches(pulseIn(pin, HIGH)));
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
void speedSet(int requiredSpeed)
{
targetSpeed = requiredSpeed;
}
void handleThrottAccel()
{
unsigned long now = micros();
if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
{
// time to check the motor speed again
if(actualSpeed < targetSpeed)
{
// accelerating
actualSpeed++;
myservoThrottle.write(actualSpeed);
}
else if(actualSpeed > targetSpeed)
{
// deccelerating
actualSpeed--;
myservoThrottle.write(actualSpeed);
}
else
{
// motor is already at the target speed - no action required
}
lastSpeedUpdateTime = now;
}
}
void handleSteerAccel()
{
unsigned long now = micros();
if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
{
// time to check the motor speed again
if(actualSpeed < targetSpeed)
{
// accelerating
actualSpeed++;
myservoSteering.write(actualSpeed);
}
else if(actualSpeed > targetSpeed)
{
// deccelerating
actualSpeed--;
myservoSteering.write(actualSpeed);
}
else
{
// motor is already at the target speed - no action required
}
lastSpeedUpdateTime = now;
}
}
void calcPirSensors()
{
}
Thomas