Roomba_Nerf_02.ino

// TBHS 2015 Episode 200 - 202 - 205 Roomba Nerf
// This code is intended for an arduino connected to a pixy and a roomba
// pixy will search for a green nerf dart, then relay the x coordinate to the arduino,
// the arduino will send instructios to the roomba for driving the motors to turn towards the nerf dart.
//
// Currently working on code to turn on and of brush and vacuum motors.

#include <SoftwareSerial.h>
#include <SPI.h>  
#include <Pixy.h>

#define rxPin 10
#define txPin 9

#define start  128
#define reset  7
#define stop  173
#define safe  131
#define full  132

#define  bumpRight  1 << 0
#define  bumpLeft   1 << 1
#define  dropRight  1 << 2
#define  dropLeft   1 << 3

#define primary 0
#define secondary 1
#define dump 2

// setup pixy
Pixy pixy;

// set up a new software serial port
SoftwareSerial roombaSerial = SoftwareSerial(rxPin, txPin); 

// pin assignments
int statusLED = 4;

// pixy variables
int deadZone = 15;
int turning = 0;

// motor variables
signed short mainBrush = 0;
signed short sideBrush = 0;
signed short vacuum = 0;

// interrupt variables
int action_0=0;
int action_1=0;
volatile int state_0 = LOW;
volatile int state_1 = LOW;
long debouncing_time = 40; //Debouncing Time in Milliseconds
volatile unsigned long last_micros;

void setup() { 
   
   // attach interrupt buttons:
   attachInterrupt(0, interrupt_0, RISING); // RISING FALLING CHANGING
   attachInterrupt(1, interrupt_1, RISING);
   
   // define pin modes for status led and direct device/baud:
   pinMode(statusLED, OUTPUT); 
   
   // set the data rate for the SoftwareSerial port, this is the
   // iRobot's default rate of 115200 baud:
   roombaSerial.begin(115200);
   
   // start hardware serial port 
   Serial.begin(115200);
   
   // define pin modes for software tx, rx pins:
   pinMode(rxPin, INPUT);
   pinMode(txPin, OUTPUT);
   
   // initalize the pixy
   pixy.init();
   Serial.println("PIXY GO START!");
   
   // begin roomba mode
   startMode(safe);
   //startMode(full);
} 

void loop() {
   
   // pixyTest_00();
   //pixyTest_01();
   motorsTest();
   //dartCollectorTest();
}

void dartCollectorTest() {
   pwmMotors(primary, 30);
   
   directDrive(250, 250);
   delay(1000);
   directDrive(0, 0);
//    delay(2000);
//    directDrive(-250, -250);
//    delay(2000);
//    directDrive(0, 0);
//    delay(2000);
   
}
void motorsTest() {
   pwmMotors(primary, 31);
   delay(2000);
   pwmMotors(primary, 0);
   delay(1000);
   pwmMotors(primary, -31);
   delay(2000);
   pwmMotors(primary, 0);
   pwmMotors(secondary, 31);
   delay(2000);
   pwmMotors(secondary, 0);
   delay(1000);
   pwmMotors(secondary, -31);
   delay(1000);
   pwmMotors(secondary, 0);
   pwmMotors(dump, 31);
   delay(2000);
   pwmMotors(dump, 0);
}

void pixyTest_01() {
   
   if (pixy.getBlocks()) {
      
      int xx = pixy.blocks[0].x;
      
      if (xx < (160 - deadZone)) {
         if (turning != 1) {
            turning = 1;
            Serial.print("\tLeft");
            Serial.print("\t");
            Serial.print(turning);
            Serial.print("\n");
            left(100);
         }
      }
      if (xx > (160 + deadZone)) {
         if (turning != 2) {
            turning = 2;
            Serial.print("\tRight");
            Serial.print("\t");
            Serial.print(turning);
            Serial.print("\n");
            right(100);
         }
      }     
      if (xx > (159 - deadZone) and xx < (161 + deadZone)) {
         if (turning != 0) {
            turning = 0;
            Serial.print("\tHalt");
            Serial.print("\t");
            Serial.print(turning);
            Serial.print("\n");
            halt();
         }
      }   
      else {
         if (turning == 0) {
            Serial.print("\t--H");
            Serial.print("\t");
            Serial.print(turning);
            Serial.print("\n");
         }
      }      
   }

}

void pixyTest_00() {
   
   if (pixy.getBlocks()) {
      
      if (pixy.blocks[0].signature == 1) {
         
         int xx = pixy.blocks[0].x;
         
         if (xx < (160 - deadZone)) {
            Serial.print("\tLeft\n"); 
         }
         if (xx > (160 + deadZone)) {
            Serial.print("\tRight\n");
         }     
         if (xx > (159 - deadZone) and xx < (161 + deadZone)) {
            Serial.print("\tHalt\n");  
         }      
         
      }
      else {
         Serial.print("\tHalt\n");  
      }    
   } 
}

void pixyLoopTest() {
   
   if (pixy.getBlocks())
   {        
      if (pixy.blocks[0].signature == 1); { 
         Serial.print("\t");
         Serial.print(pixy.blocks[0].x);
         Serial.print("\t\t");
         Serial.println(pixy.blocks[0].y);    
      }      
   }  
}

void directDrive(signed short leftV, signed short rightV) {
   
   roombaSerial.write(145); // Opcode number for DIRECT DRIVE
   roombaSerial.write(rightV >> 8); 
   roombaSerial.write(rightV & 0xFF);  
   roombaSerial.write(leftV >> 8); 
   roombaSerial.write(leftV & 0xFF); 
   
}

void halt() {
   directDrive(0,0);
}

void left(signed short velocity) {
   directDrive(0-velocity,velocity);
}

void right(signed short velocity) {
   directDrive(velocity,0-velocity);
}

void startMode(signed short mode) {
   
   // light up status led   
   digitalWrite(statusLED, HIGH); 
   
   // this instruction starts the OI, roomba must
   // always recieve the Start instruction before
   // it will acknowladge any other operations
   roombaSerial.write(start);  // Start the roomba OI
   roombaSerial.write(mode);
   
   blinkLED();
}

void blinkLED() {
   // blink status LED  
   digitalWrite(statusLED, HIGH); 
   delay(100);
   digitalWrite(statusLED, LOW);  
   delay(100);
   digitalWrite(statusLED, HIGH); 
   delay(100);
   digitalWrite(statusLED, LOW);  
   delay(100);
}

void pwmMotors(signed short motor, signed short pulseWidthModulation) {
   // assign global variables
   switch (motor) {
      case primary:
         mainBrush = pulseWidthModulation;    // range from -127 to 127
         break;
      case secondary:
         sideBrush = pulseWidthModulation;    // range from -127 to 127
         break;
      case dump:
         vacuum = pulseWidthModulation;       // range from 0 to 127
         break;
      default: 
         break;
   }
   // send instructions
   roombaSerial.write(144);        // OI opcode for PWM Motors
   // roombaSerial.write((byte)mainBrush);  
   roombaSerial.write(mainBrush);  
   // roombaSerial.write((byte)sideBrush);
   roombaSerial.write(sideBrush);
   // roombaSerial.write((byte)vacuum);
   roombaSerial.write(vacuum);
   
   Serial.println("\tmainBrush\tsideBrush\tvacuum");
   Serial.print("\t");
   Serial.print(mainBrush);
   Serial.print(",");
   Serial.print((byte)mainBrush);
   Serial.print("\t\t");
   Serial.print(sideBrush);
   Serial.print(",");
   Serial.print((byte)sideBrush);
   Serial.print("\t\t");
   Serial.print(vacuum);
   Serial.print(",");
   Serial.print((byte)vacuum);
   Serial.print("\n");
   
   // Serial.println((byte)mainBrush);
   // Serial.println("sideBrush");
   // Serial.println(sideBrush);
   // Serial.println((byte)sideBrush);
   // Serial.println("vacuum");
   // Serial.println(vacuum);
   // Serial.println((byte)vacuum);
}

void interrupt_0() {
   // debounce
   if((long)(micros() - last_micros) >= debouncing_time * 1000) {
      // Do Something
      last_micros = micros();
   }
   // state_0 = !state_0;
   digitalWrite(statusLED, HIGH); 
   delay(50);
   digitalWrite(statusLED, LOW);  
   delay(50);
   
   action_0 = 0;
   // Do Something
}

void interrupt_1() {
   // debounce
   if((long)(micros() - last_micros) >= debouncing_time * 1000) {
      // Do Something
      last_micros = micros();
   }
   
   // state_1 = !state_1;
   digitalWrite(statusLED, HIGH); 
   delay(50);
   digitalWrite(statusLED, LOW);  
   delay(50);
   
   if (action_0 < 7){
      action_0++;
   }
   else {
      action_0 = 1;
   }

   
}