Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
BlueStamp-TabletopRobot Final
//Include and create objects within libraries
#include <Servo.h>
#include <SoftwareSerial.h>
Servo servo1;
Servo servo2;
SoftwareSerial XBee(2, 3);
//Declare pin references & other constants
int servo1Pin = 9;
int servo2Pin = 10;
int ledPin = 8;
int IRemitter1Pin = 11;
int IRemitter2Pin = 12;
int IRdetector1Pin = 6;
int IRdetector2Pin = 7;
int PULSE = 38000;
int SERVO_SPEED = 10;
int CYCLE = 1000;
String lastSent;
int XBEE_SEND_FREQ = 200;
int IR_CHECK_FREQ = 40;
double pie = 3.141592653589792;
double direct = 0;
double x = 0;
double y = 0;
void setup() {
Serial.begin(9600);
XBee.begin(9600);
pinMode(servo1Pin, OUTPUT);
pinMode(servo2Pin, OUTPUT);
pinMode(IRdetector1Pin, INPUT);
pinMode(IRdetector2Pin, INPUT);
pinMode(ledPin, OUTPUT);
servo1.attach(servo1Pin);
servo2.attach(servo2Pin);
randomSeed(analogRead(0));
servo1.write(90 + SERVO_SPEED);
servo2.write(90 - SERVO_SPEED);
}
void loop(){
for(int i = 0; i < CYCLE; i++){ //Using a for loop instead of delay() so that I can run other code at the same time.
if(i % IR_CHECK_FREQ == 0){ // Run for every IR_CHECK_FREQ because the sensor cannot handle long duration of IR light.
IRcheck(IRemitter1Pin, IRdetector1Pin, servo1, servo2, 1); //Check 1st pair of IR and go backward a bit if the sensor does not detect the IR reflected from table.
IRcheck(IRemitter2Pin, IRdetector2Pin, servo2, servo1, -1); //Same thing but for the second pair.
}
if(i % XBEE_SEND_FREQ == 0){
//Serial.print((int)(x * 100));
XBee.print ((int)(y / 100));
XBee.print(",");
XBee.print((int)(x / 100));
XBee.print(".");
// XBee.print(direct);
// XBee.println(".");
}
if (XBee.available()){
remoteCheck(servo2, servo1, XBee.read());
}
x += cos(direct / 180 * pie) * SERVO_SPEED;
y += sin(direct / 180 * pie) * SERVO_SPEED;
delay(1); //Wait one mS to record time for the loop
/*
Serial.print("Servo1:");
Serial.println((int)servo1.read());
Serial.print("Servo2:");
Serial.println((int)servo2.read());
*/
}
}
void remoteCheck(Servo servoLeft,Servo servoRight, char c){
switch (c){
case 'w': // If received 'w'
case 'W': // or 'W'
servoLeft.write(90 + SERVO_SPEED);
servoRight.write(90 - SERVO_SPEED);
if(direct > 359){
direct -= 360;
}
break;
case 'a': // If received 'a'
case 'A': // or 'A'b
turn(servoLeft, servoRight, -1);
break;
case 's': // If received 's'
case 'S': // or 'S'
servoLeft.write(90 - SERVO_SPEED);
servoRight.write(90 + SERVO_SPEED);
direct += 180;
if(direct > 359){
direct -= 360;
}
break;
case 'd': // If received 'd'
case 'D': // or 'D'
turn(servoRight, servoLeft, 1);
break;
}
}
void IRcheck(int emitter, int detector, Servo servo, Servo otherServo, int turnDirect){
tone(emitter, PULSE); //Tone the emitter to the frequency the detector can detect.
delay(1); //Give time for detector to react;
if(digitalRead(detector)) turn(servo, otherServo, turnDirect); //If not detect IR reflected from the table, react by going back and turning.
noTone(emitter); //Stop tonning to give detector a rest and let the other pair check IR.
}
void turn(Servo backServo, Servo otherServo, int turnDirect){
int ogBackServoSpeed = (int)backServo.read(); //Save the original speed of servos.
int ogOtherServoSpeed = (int)otherServo.read();
otherServo.write(90); //Stop the not related servo from moving.
backServo.write(180 - ogBackServoSpeed); //Turn the servo to rotate in the opposite direction;
int rdm = random(40, 1000); //Generate a random number.
direct = (direct + SERVO_SPEED * rdm / 138 * turnDirect) / 360;
if(direct > 360) direct -= 360;
delay(rdm); //Keep the servo moving the other direction for a random amount of time
otherServo.write(ogOtherServoSpeed); //Load back the original speed of servos.
backServo.write(ogBackServoSpeed);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.