Capture Robots

From STEAMwiki
Jump to: navigation, search


Robot Control HTML Pages:

Particle Number Control HTML Page
67 https://thimbleprojects.org/steamlabs/93018
69 https://thimbleprojects.org/steamlabs/93019
71 https://thimbleprojects.org/steamlabs/93016
72 https://thimbleprojects.org/steamlabs/93020
73 https://thimbleprojects.org/steamlabs/93017



Robot source code:


// Servos
Servo servoArr[4]; //Create 4 servo objects

// Debug mode. Set to true to see serial output debugging
bool debugOn = false;


void setup() {
    // Drive motors
    pinMode(A0, OUTPUT);
    pinMode(A1, OUTPUT);
    pinMode(A2, OUTPUT);
    pinMode(A3, OUTPUT);
    Particle.function("goForwards", goForwards);
    Particle.function("goLeft", goLeft);
    Particle.function("goRight", goRight);
    Particle.function("goBack", goBack);
    Particle.function("goStop", goStop);


    // Servos
    servoArr[0].attach(D0);
    servoArr[1].attach(D1);
    servoArr[2].attach(D2);
    servoArr[3].attach(D3);
    Particle.function("servoControl", servoControl);
    
    // Motor Outputs
    pinMode(D4, OUTPUT);
    pinMode(D5, OUTPUT);
    pinMode(D6, OUTPUT);
    pinMode(D7, OUTPUT);
    Particle.function("motorControl", motorControl);
   
    
    if (debugOn) {
        Serial.begin(9600);
    }
    
}

void loop() {
    delay(5);
}

void debug(String message) {
    // Usage:  debug(String(motorPins[0]));
    
    if (debugOn) {
        Serial.println(message);
    }
}

int goForwards (String command) {
    digitalWrite(A0, LOW);
    digitalWrite(A1, HIGH);
    digitalWrite(A2, HIGH);
    digitalWrite(A3, LOW);
    return 1;
}

int goLeft (String command) {
    digitalWrite(A0, LOW);
    digitalWrite(A1, HIGH);
    digitalWrite(A2, LOW);
    digitalWrite(A3, HIGH);
    return 1;
}

int goRight (String command) {
    digitalWrite(A0, HIGH);
    digitalWrite(A1, LOW);
    digitalWrite(A2, HIGH);
    digitalWrite(A3, LOW);
    return 1;
}

int goBack (String command) {
    digitalWrite(A0, HIGH);
    digitalWrite(A1, LOW);
    digitalWrite(A2, LOW);
    digitalWrite(A3, HIGH);
    return 1;
}

int goStop (String command) {
    digitalWrite(A0, LOW);
    digitalWrite(A1, LOW);
    digitalWrite(A2, LOW);
    digitalWrite(A3, LOW);
    return 1;
}

int servoControl(String command)
{
   debug(command);
    // Parse the command into servo number and angle
    // Message format: "1-90" means turn servo 1 to 90 degrees
   int servoNumber = command.substring(0,1).toInt() -1;
   int servoPosition = command.substring(2).toInt();
   
   // Make sure it is in the right range
   // And set the position
   servoPosition = constrain( servoPosition, 0 , 360);

   // Set the servo
   servoArr[servoNumber].write( servoPosition );
    
  // done
   return 1;
}

int motorControl (String command) {
    debug(command);
    
    // Parse the command into servo number and angle
    // Message format: "1-F" means turn motor 1 forwards
    // F = Forwards
    // B = Backwards
    // S = Stop
    
    int motorNumber = command.substring(0,1).toInt() -1;
    String motorDirection = command.substring(2);
    int motorPins[2];
    if (motorNumber == 1) {
        motorPins[0] = D4;
        motorPins[1] = D5;
    } else {
        motorPins[0] = D6;
        motorPins[1] = D7;

    }
    debug(String(motorPins[0]));
    
    if (motorDirection == "F") {
        digitalWrite(motorPins[0], HIGH);
        digitalWrite(motorPins[1], LOW);
    } else if (motorDirection == "B" || motorDirection == "R") {
        digitalWrite(motorPins[0], LOW);
        digitalWrite(motorPins[1], HIGH);
    } else if (motorDirection == "S") {
        digitalWrite(motorPins[0], LOW);
        digitalWrite(motorPins[1], LOW);
    } 
   
   
    return 1;
}