Initial Commit

ArmSlaveCommands/ArmSlaveCommands.ino

@@ -0,0 +1,207 @@
+/*
+ * ArmSlaveCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for ArmSlaveCommands ----------
+#include <Wire.h>
+#include <Servo.h>
+
+// Pin Out --------------------------------------------------
+const int rotatePin = 11;
+const int sholderPin = 10;
+const int elbowPin = 6;
+const int wristPin = 9;
+const int wrist_rotatePin = 5;
+const int gripPin = 3;
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 0;
+
+// Changing Variables ---------------------------------------
+int rot = 0;
+int pos1 = 0;
+int pos2 = 0;
+int x = 10; 
+
+// Class Objects --------------------------------------------
+Servo servo_rotate;
+Servo servo_shoulder;
+Servo servo_elbow;
+Servo servo_wrist;
+Servo servo_wrist_rotate;
+Servo servo_grip;
+
+// Intial Setup ---------------------------------------------
+void setup() {
+  Wire.begin(8); // join i2c bus with address #8
+  servo_rotate.attach(rotatePin);
+  servo_shoulder.attach(sholderPin);
+  servo_elbow.attach(elbowPin);
+  servo_wrist.attach(wristPin);
+  servo_wrist_rotate.attach(wrist_rotatePin);
+  servo_grip.attach(gripPin);
+  //set arm to start position
+  resetArm();
+  
+  Wire.onReceive(receiveEvent); // register event
+  Serial.begin(115200);
+}
+
+// Main Loop ------------------------------------------------
+void loop() {
+
+  //delay(100);
+}
+
+// Functions ------------------------------------------------
+void receiveEvent(int howMany) { // takes in the number of bytes from master 
+  /*
+  * This will take in the arm commands and run the servos
+  * Function runs whenever Uno receives a transmission
+  */
+  
+  //changed from while loop to if statement might need to change for press vs click
+  if (Wire.available()) { // loop through all
+  
+   x = Wire.read();    // receive byte as an integer
+   moveArm();
+   if(DEBUG) {
+    Serial.print("------Arm Command-----\n");
+    Serial.print(x); 
+    Serial.print("\n");
+    }
+  }
+}
+  
+void moveArm(void){
+  
+  if (x == 0){
+      //UP
+      //open the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2++;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 1){
+      //DOWN
+      //close the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2--;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 2){
+      //RIGHT
+      //pivot arm right
+      rot = servo_rotate.readMicroseconds();
+      rot--;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if (x == 3){
+      //LEFT
+      //pivot arm left
+      rot = servo_rotate.readMicroseconds();
+      rot++;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if(x == 4){
+      //TRIANGLE
+      //move shoulder up
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1--;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 5){
+      //CROSS
+      //move shoulder down
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1++;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 6){
+      //L1
+      //open gripper
+      servo_grip.writeMicroseconds(1100);
+    }
+
+    if(x == 7){
+      //R1
+      //close gripper
+      servo_grip.writeMicroseconds(1550);
+    }
+    if(x == 8){
+      //CIRCLE
+      //move wrist up
+      pos1 = servo_wrist.readMicroseconds();
+      pos1++;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+    if( x == 9){
+      //SQUARE
+      //move wrist down
+      pos1 = servo_wrist.readMicroseconds();
+      pos1--;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+
+    if (x == 10){
+      //OPTIONS
+      //reset to initial position
+      resetArm();
+    }
+
+    if(x==11){
+      //SHARE
+      //print current positions in microseconds
+      Serial.print("---------------------------\n");
+      Serial.print("Rotate: ");
+      Serial.print(servo_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Shoulder: ");
+      Serial.print(servo_shoulder.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Elbow: ");
+      Serial.print(servo_elbow.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Wrist: ");
+      Serial.print(servo_wrist.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Write Rotate: ");
+      Serial.print(servo_wrist_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Grip: ");
+      Serial.print(servo_grip.readMicroseconds());
+      Serial.print("\n");
+    }
+}
+
+void resetArm(void){
+  //writing initial position
+  servo_rotate.writeMicroseconds(2000);
+  //544 straight up
+  servo_shoulder.writeMicroseconds(544);
+  servo_elbow.writeMicroseconds(1500);
+  servo_wrist.writeMicroseconds(1500);
+  servo_wrist_rotate.writeMicroseconds(1500);
+  servo_grip.writeMicroseconds(1550);
+}
+

ArmSlaveCommands/ArmSlaveCommands3.2.ino

@@ -0,0 +1,300 @@
+/*
+ * ArmSlaveCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for ArmSlaveCommands ----------
+#include <Wire.h>
+#include <Servo.h>
+
+// Pin Out --------------------------------------------------
+const int rotatePin = 11;
+const int sholderPin = 10;
+const int elbowPin = 9;
+const int wristPin = 5;
+const int wrist_rotatePin = 6;
+const int gripPin = 3;
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 0;
+
+// Changing Variables ---------------------------------------
+int rot = 0;
+int pos1 = 0;
+int pos2 = 0;
+int x = 10; 
+
+// Class Objects --------------------------------------------
+Servo servo_rotate;
+Servo servo_shoulder;
+Servo servo_elbow;
+Servo servo_wrist;
+Servo servo_wrist_rotate;
+Servo servo_grip;
+
+// Intial Setup ---------------------------------------------
+void setup() {
+  Wire.begin(8); // join i2c bus with address #8
+
+  //startUpArm();
+  servo_shoulder.attach(sholderPin);
+  servo_rotate.attach(rotatePin);
+  servo_elbow.attach(elbowPin);
+  servo_wrist.attach(wristPin);
+  servo_wrist_rotate.attach(wrist_rotatePin); 
+  servo_grip.attach(gripPin);
+
+  //set arm to start position
+  resetArm();
+  
+
+  Wire.onReceive(receiveEvent); // register event
+  Serial.begin(115200);
+}
+
+// Main Loop ------------------------------------------------
+void loop() {
+
+  //delay(100);
+}
+
+// Functions ------------------------------------------------
+void receiveEvent(int howMany) { // takes in the number of bytes from master 
+  /*
+  * This will take in the arm commands and run the servos
+  * Function runs whenever Uno receives a transmission
+  */
+  
+  //changed from while loop to if statement might need to change for press vs click
+  if (Wire.available()) { // loop through all
+  
+   x = Wire.read();    // receive byte as an integer
+   moveArm();
+   if(DEBUG) {
+    Serial.print("------Arm Command-----\n");
+    Serial.print(x); 
+    Serial.print("\n");
+    }
+  }
+}
+  
+void moveArm(void){
+  
+  if (x == 0){
+      //TRIANGLE
+      //open the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2++;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 1){
+      //SQUARE
+      //close the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2--;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 2){
+      //RIGHT
+      //pivot arm right
+      rot = servo_rotate.readMicroseconds();
+      rot--;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if (x == 3){
+      //LEFT
+      //pivot arm left
+      rot = servo_rotate.readMicroseconds();
+      rot++;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if(x == 4){
+      //UP
+      //move shoulder up
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1--;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 5){
+      //DOWN
+      //move shoulder down
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1++;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 6){
+      //L1
+      //open gripper
+      servo_grip.writeMicroseconds(1100);
+    }
+
+    if(x == 7){
+      //R1
+      //close gripper
+      servo_grip.writeMicroseconds(1575); //Was 1550
+    }
+    if(x == 8){
+      //CIRCLE
+      //move wrist up
+      pos1 = servo_wrist.readMicroseconds();
+      pos1++;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+    if( x == 9){
+      //CROSS
+      //move wrist down
+      pos1 = servo_wrist.readMicroseconds();
+      pos1--;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+
+    if (x == 10){
+      //OPTIONS
+      //reset to initial position
+      resetArm();
+    }
+
+    if(x==11){
+      //SHARE
+      //print current positions in microseconds
+      Serial.print("---------------------------\n");
+      Serial.print("Rotate: ");
+      Serial.print(servo_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Shoulder: ");
+      Serial.print(servo_shoulder.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Elbow: ");
+      Serial.print(servo_elbow.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Wrist: ");
+      Serial.print(servo_wrist.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Write Rotate: ");
+      Serial.print(servo_wrist_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Grip: ");
+      Serial.print(servo_grip.readMicroseconds());
+      Serial.print("\n");
+    }
+    if(x == 12){
+      pos1 = servo_shoulder.readMicroseconds();
+      if(pos1 < 1800){
+        pos1++;
+        servo_shoulder.writeMicroseconds(pos1);
+      }else if(pos1 > 1800){
+        pos1--;
+        servo_shoulder.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_elbow.readMicroseconds();
+      if(pos1 < 1879){
+        pos1++;
+        servo_elbow.writeMicroseconds(pos1);
+      }else if(pos1 > 1879){
+        pos1--;
+        servo_elbow.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_wrist.readMicroseconds();
+      if(pos1 < 1434){
+        pos1++;
+        servo_wrist.writeMicroseconds(pos1);
+      }else if(pos1 > 1434){
+        pos1--;
+        servo_wrist.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_rotate.readMicroseconds();
+      if(pos1 < 2245){
+        pos1++;
+        servo_rotate.writeMicroseconds(pos1);
+      }else if(pos1 > 2245){
+        pos1--;
+        servo_rotate.writeMicroseconds(pos1);
+      }
+      
+    }
+
+      if(x == 20){
+        servo_shoulder.writeMicroseconds(1500);
+      }
+
+      if(x == 21){
+        servo_elbow.writeMicroseconds(1879);
+      }
+
+      if(x == 22){
+        servo_wrist.writeMicroseconds(1434);
+      }
+
+      if(x == 23){
+        servo_rotate.writeMicroseconds(2245);
+      }
+
+      if(x >= 24){
+        int k = servo_shoulder.readMicroseconds();
+        if(k < 2245){
+          servo_shoulder.writeMicroseconds(k+100);
+        }
+      }
+}
+
+void resetArm(void){
+  //writing initial position
+  servo_rotate.writeMicroseconds(2000);
+  //544 straight up
+  servo_shoulder.writeMicroseconds(1500);
+  servo_elbow.writeMicroseconds(1500);
+  servo_wrist.writeMicroseconds(1500);
+  servo_wrist_rotate.writeMicroseconds(1500);
+  servo_grip.writeMicroseconds(1550);
+}
+
+void startUpArm(void){
+
+  servo_shoulder.attach(sholderPin);
+  for(int i = 0; i < 1800; i++) {
+  servo_shoulder.writeMicroseconds(1800);
+  delay(700);
+  }
+  
+  servo_elbow.attach(elbowPin);
+  servo_elbow.writeMicroseconds(1500);
+  delay(700);
+  
+  servo_rotate.attach(rotatePin);
+  servo_rotate.writeMicroseconds(2400);
+  delay(700);
+
+  servo_wrist.attach(wristPin);
+  servo_wrist.writeMicroseconds(800);
+
+  servo_wrist_rotate.attach(wrist_rotatePin); 
+  servo_wrist_rotate.writeMicroseconds(1500);
+  
+  servo_grip.attach(gripPin);
+  servo_grip.writeMicroseconds(1550);
+
+  servo_elbow.writeMicroseconds(1200);
+}
+

ArmSlaveCommands/ArmSlaveCommands3.3.ino

@@ -0,0 +1,342 @@
+/*
+ * ArmSlaveCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for ArmSlaveCommands ----------
+#include <Wire.h>
+#include <Servo.h>
+
+// Pin Out --------------------------------------------------
+const int rotatePin = 11;
+const int sholderPin = 10;
+const int elbowPin = 9;
+const int wristPin = 5;
+const int wrist_rotatePin = 6;
+const int gripPin = 3;
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 0;
+
+// Changing Variables ---------------------------------------
+int rot = 0;
+int pos1 = 0;
+int pos2 = 0;
+int x = 10; 
+
+// Class Objects --------------------------------------------
+Servo servo_rotate;
+Servo servo_shoulder;
+Servo servo_elbow;
+Servo servo_wrist;
+Servo servo_wrist_rotate;
+Servo servo_grip;
+
+// Intial Setup ---------------------------------------------
+void setup() {
+  Wire.begin(8); // join i2c bus with address #8
+
+  //startUpArm();
+  servo_shoulder.attach(sholderPin);
+  servo_rotate.attach(rotatePin);
+  servo_elbow.attach(elbowPin);
+  servo_wrist.attach(wristPin);
+  servo_wrist_rotate.attach(wrist_rotatePin); 
+  servo_grip.attach(gripPin);
+
+  //set arm to start position
+  resetArm();
+  
+
+  Wire.onReceive(receiveEvent); // register event
+  Serial.begin(115200);
+}
+
+// Main Loop ------------------------------------------------
+void loop() {
+
+  //delay(100);
+}
+
+// Functions ------------------------------------------------
+void receiveEvent(int howMany) { // takes in the number of bytes from master 
+  /*
+  * This will take in the arm commands and run the servos
+  * Function runs whenever Uno receives a transmission
+  */
+  
+  //changed from while loop to if statement might need to change for press vs click
+  if (Wire.available()) { // loop through all
+  
+   x = Wire.read();    // receive byte as an integer
+   moveArm();
+   if(DEBUG) {
+    Serial.print("------Arm Command-----\n");
+    Serial.print(x); 
+    Serial.print("\n");
+    }
+  }
+}
+  
+void moveArm(void){
+  
+  if (x == 0){
+      //TRIANGLE
+      //open the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2++;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 1){
+      //SQUARE
+      //close the elbow
+      pos2 = servo_elbow.readMicroseconds();
+      pos2--;
+      servo_elbow.writeMicroseconds(pos2);
+      
+    }
+
+    if (x == 2){
+      //RIGHT
+      //pivot arm right
+      rot = servo_rotate.readMicroseconds();
+      rot--;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if (x == 3){
+      //LEFT
+      //pivot arm left
+      rot = servo_rotate.readMicroseconds();
+      rot++;
+      servo_rotate.writeMicroseconds(rot);
+      
+    }
+
+    if(x == 4){
+      //UP
+      //move shoulder up
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1--;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 5){
+      //DOWN
+      //move shoulder down
+      pos1 = servo_shoulder.readMicroseconds();
+      pos1++;
+      servo_shoulder.writeMicroseconds(pos1);
+      
+    }
+
+    if(x == 6){
+      //L1
+      //open gripper
+      servo_grip.writeMicroseconds(1100);
+    }
+
+    if(x == 7){
+      //R1
+      //close gripper
+      servo_grip.writeMicroseconds(1650); //Was 1550
+    }
+    if(x == 8){
+      //CIRCLE
+      //move wrist up
+      pos1 = servo_wrist.readMicroseconds();
+      pos1++;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+    if( x == 9){
+      //CROSS
+      //move wrist down
+      pos1 = servo_wrist.readMicroseconds();
+      pos1--;
+      servo_wrist.writeMicroseconds(pos1);
+    }
+
+    if (x == 10){
+      //OPTIONS
+      //reset to initial position
+      resetArm();
+    }
+
+    if(x==11){
+      //SHARE
+      //print current positions in microseconds
+      Serial.print("---------------------------\n");
+      Serial.print("Rotate: ");
+      Serial.print(servo_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Shoulder: ");
+      Serial.print(servo_shoulder.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Elbow: ");
+      Serial.print(servo_elbow.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Wrist: ");
+      Serial.print(servo_wrist.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Write Rotate: ");
+      Serial.print(servo_wrist_rotate.readMicroseconds());
+      Serial.print("\n");
+
+      Serial.print("Grip: ");
+      Serial.print(servo_grip.readMicroseconds());
+      Serial.print("\n");
+    }
+
+    // Streatched out arm for things
+    if(x == 12){
+      pos1 = servo_shoulder.readMicroseconds();
+      if(pos1 < 1800){
+        pos1++;
+        servo_shoulder.writeMicroseconds(pos1);
+      }else if(pos1 > 1800){
+        pos1--;
+        servo_shoulder.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_elbow.readMicroseconds();
+      if(pos1 < 1879){
+        pos1++;
+        servo_elbow.writeMicroseconds(pos1);
+      }else if(pos1 > 1879){
+        pos1--;
+        servo_elbow.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_wrist.readMicroseconds();
+      if(pos1 < 1434){
+        pos1++;
+        servo_wrist.writeMicroseconds(pos1);
+      }else if(pos1 > 1434){
+        pos1--;
+        servo_wrist.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_rotate.readMicroseconds();
+      if(pos1 < 2245){
+        pos1++;
+        servo_rotate.writeMicroseconds(pos1);
+      }else if(pos1 > 2245){
+        pos1--;
+        servo_rotate.writeMicroseconds(pos1);
+      }
+      
+    }
+
+     // Autonoumous arm position for dropping hacky sack
+     if(x == 13){
+      pos1 = servo_shoulder.readMicroseconds();
+      if(pos1 < 1000){
+        pos1++;
+        servo_shoulder.writeMicroseconds(pos1);
+      }else if(pos1 > 1000){
+        pos1--;
+        servo_shoulder.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_elbow.readMicroseconds();
+      if(pos1 < 544){
+        pos1++;
+        servo_elbow.writeMicroseconds(pos1);
+      }else if(pos1 > 544){
+        pos1--;
+        servo_elbow.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_wrist.readMicroseconds();
+      if(pos1 < 1260){
+        pos1++;
+        servo_wrist.writeMicroseconds(pos1);
+      }else if(pos1 > 1260){
+        pos1--;
+        servo_wrist.writeMicroseconds(pos1);
+      }
+
+      pos1 = servo_rotate.readMicroseconds();
+      if(pos1 < 2238){
+        pos1++;
+        servo_rotate.writeMicroseconds(pos1);
+      }else if(pos1 > 2238){
+        pos1--;
+        servo_rotate.writeMicroseconds(pos1);
+      }
+      
+    }
+
+      if(x == 20){
+        servo_shoulder.writeMicroseconds(1500);
+      }
+
+      if(x == 21){
+        servo_elbow.writeMicroseconds(1879);
+      }
+
+      if(x == 22){
+        servo_wrist.writeMicroseconds(1434);
+      }
+
+      if(x == 23){
+        servo_rotate.writeMicroseconds(2245);
+      }
+
+      if(x >= 24){
+        int k = servo_shoulder.readMicroseconds();
+        if(k < 2245){
+          servo_shoulder.writeMicroseconds(k+100);
+        }
+      }
+}
+
+void resetArm(void){
+  //writing initial position
+  servo_rotate.writeMicroseconds(2238);
+  //544 straight up
+  servo_shoulder.writeMicroseconds(1184);
+  servo_elbow.writeMicroseconds(544);
+  servo_wrist.writeMicroseconds(1260);
+  servo_wrist_rotate.writeMicroseconds(1500);
+  servo_grip.writeMicroseconds(1100);
+}
+
+//void startUpArm(void){
+//
+//  servo_shoulder.attach(sholderPin);
+//  for(int i = 0; i < 1800; i++) {
+//  servo_shoulder.writeMicroseconds(1800);
+//  delay(700);
+//  }
+//  
+//  servo_elbow.attach(elbowPin);
+//  servo_elbow.writeMicroseconds(1500);
+//  delay(700);
+//  
+//  servo_rotate.attach(rotatePin);
+//  servo_rotate.writeMicroseconds(2400);
+//  delay(700);
+//
+//  servo_wrist.attach(wristPin);
+//  servo_wrist.writeMicroseconds(800);
+//
+//  servo_wrist_rotate.attach(wrist_rotatePin); 
+//  servo_wrist_rotate.writeMicroseconds(1500);
+//  
+//  servo_grip.attach(gripPin);
+//  servo_grip.writeMicroseconds(1550);
+//
+//  servo_elbow.writeMicroseconds(1200);
+//}
+

ArmSlaveCommands/desktop.ini

@@ -0,0 +1,5 @@
+[.ShellClassInfo]
+InfoTip=This folder is shared online.
+IconFile=C:\Program Files\Google\Drive\googledrivesync.exe
+IconIndex=16
+    

RobotCommands/RobotCommands.ino

@@ -0,0 +1,302 @@
+/*
+ * RobotCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for RobotCommands ----------
+#include <Wire.h>
+#include <PS4BT.h>
+#include <usbhub.h>
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+#include <SPI.h>
+
+
+// Pin Out --------------------------------------------------
+const int pingPin = 7;
+const int motorPins[4] = {3,5,6,7};
+const int directionPins[4] = {22,24,26,28};
+
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 0; 
+const int neutral = 127;
+const int forward = 254;
+
+// Changing Variables ---------------------------------------
+//Robots
+unsigned char irReadings[8];
+int motorSpeeds[4];
+unsigned long cm;
+int drive = 0;
+int strafe = 0;
+int rotate = 0;
+int armCommand = 0;//changed to int from char set to 0 instead of 10
+
+//USB Shield
+
+bool printAngle, printTouch;
+uint8_t oldL2Value, oldR2Value;
+
+// Class Objects --------------------------------------------
+USB Usb;
+BTD Btd(&Usb);
+PS4BT PS4(&Btd, PAIR);
+// PS4BT PS4(&Btd);
+
+// Intial Setup ---------------------------------------------
+
+void setup() {
+  Wire.begin(); // join I2C bus
+  Serial.begin(115200);
+  #if !defined(__MIPSEL__)
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  #endif
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
+
+  for(int i = 0; i <4; i++) {
+    pinMode(motorPins[i], OUTPUT);
+    pinMode(directionPins[i], OUTPUT);
+  }
+ 
+}
+
+// Main Loop ------------------------------------------------
+
+void loop() {
+  Usb.Task(); 
+  if (PS4.connected()) { //PS4 button checks will be in this if() statement
+
+    //Motor Controls
+    if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 
+    || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117
+    || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117
+    || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
+
+      driveMotorsWithPS4();
+    } else {
+      stopMotors();
+    } //End of PS4.getAnaloghat if() statement 
+
+    //Arm Controls
+    /*CURRENT ISSUE 05-OCT-2017
+     * When set to getButtonPress causes arm to go crazy
+     * activates more than the intended servo cause unknown
+     * when the button is release arm returns to the position it was
+     * already in before pressing the button
+     */
+    if (PS4.getButtonClick(UP)|| PS4.getButtonPress(UP)){//open the elbow
+      armCommand = 0;
+      tellArm();
+    } //End Up if() statement
+    
+    if (PS4.getButtonClick(DOWN)||PS4.getButtonPress(DOWN)){//close the elbow
+      armCommand = 1;
+      tellArm();
+    }  //End Down if() statement
+
+    if (PS4.getButtonClick(RIGHT)||PS4.getButtonPress(RIGHT)){//pivot arm right
+      armCommand = 2;
+      tellArm();
+    } //End Right if() statement
+
+    if (PS4.getButtonClick(LEFT)||PS4.getButtonPress(LEFT)){//pivot arm left
+      armCommand = 3;
+      tellArm();
+    } //End Left if() statement
+
+    if(PS4.getButtonClick(TRIANGLE)|| PS4.getButtonPress(TRIANGLE)){//move shoulder up
+      armCommand = 4;
+      tellArm();
+    } //End Triangle if() statement
+
+    if(PS4.getButtonClick(CROSS)||PS4.getButtonPress(CROSS)){//move shoulder down
+      armCommand = 5;
+      tellArm();
+    } //End Cross if() statement
+
+    if(PS4.getButtonClick(L1)){//open gripper
+      armCommand = 6;
+      tellArm();
+    } //End L1 if() statement
+
+    if(PS4.getButtonClick(R1)){//close gripper
+      armCommand = 7;
+      tellArm();
+    } //End R1 if() statement
+
+    if(PS4.getButtonClick(CIRCLE)|| PS4.getButtonPress(CIRCLE)){//raise wrist
+      armCommand = 8;
+      tellArm();
+      }
+
+    if(PS4.getButtonClick(SQUARE)||PS4.getButtonPress(SQUARE)){//lower wrist
+      armCommand = 9;
+      tellArm();
+    }
+
+    if(PS4.getButtonClick(OPTIONS)){//reset arm
+      armCommand = 10;
+      tellArm();
+    }
+
+    if(PS4.getButtonClick(SHARE)){//debug for arm
+      armCommand = 11;
+      tellArm();
+    }
+    //Sensor Controls
+    
+  }//End of PS4.connected if() statement
+}
+
+// Functions ------------------------------------------------
+// Sensor Functions
+void askIR(void){
+/*
+ * Used to update IR sensor readings.
+ * Sends I2C request to bus address #9. Asks for 16 bytes, only need 0,2,4,8,10,12,14
+ * Pass in an char array of 8 to be modified with new sensor readings, [0-255]
+ * #DEFINE irReadings
+ */
+  static unsigned char t, data[16];
+
+  t = 0;
+
+  Wire.requestFrom(9,16); // request 16 bytes from slave device #9
+  while (Wire.available()) { // slave may send less than requested
+    data[t] = Wire.read(); // receive a byte as character
+    t++;
+  }
+  for(char i = 0; i < 8; i++){
+    irReadings[i] = data[i*2];
+  }
+  
+  if(DEBUG) {
+    Serial.print("-----IR Readings-----\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(irReadings[i]); 
+    }
+    Serial.print("\n");
+  }
+}
+
+int askPing(void){
+/*
+ * Sends signal to activate Ping sensor and returns the distance in centimeters (may try using mm)
+ * Process takes 7-8 milliseconds.
+ * DEFINE pingPin
+ */
+  static unsigned long duration;
+
+  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
+  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
+  pinMode(pingPin, OUTPUT);
+  digitalWrite(pingPin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(pingPin, HIGH);
+  delayMicroseconds(5);
+  digitalWrite(pingPin, LOW);
+
+  // The same pin is used to read the signal from the PING))): a HIGH pulse
+  // whose duration is the time (in microseconds) from the sending of the ping
+  // to the reception of its echo off of an object.
+  pinMode(pingPin, INPUT);
+  duration = pulseIn(pingPin, HIGH);
+
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance of the object we
+  // take half of the distance traveled.
+  cm = duration / 29 / 2;
+  
+  if(DEBUG) {
+    Serial.print("-----Ping(cm)------\n");
+    Serial.print(cm);
+    Serial.print("\n");
+  }
+}
+
+//Arm Funcitons
+
+void tellArm(void){
+/*
+ * Should send the desired locations of the arm to the Arduino Uno over I2C at address #8
+ * Format of the data needs to be determined. Speed,Joint,Degree in a single signal or in 6 joint
+ * intervals with speeds being controlled by analog pin or something
+ */
+
+    Wire.beginTransmission(8); // transmit to device #8
+    Wire.write(armCommand);
+    Wire.endTransmission(); // stop transmitting\
+    
+    if(DEBUG) {
+      Serial.print("-----Arm Command----\n");
+      Serial.print(armCommand);
+      Serial.print("\n");
+    }
+}
+
+//Motor Functions
+
+void tellMotors(void){
+/*
+ * Uses the [(-)255-255] speed variables and sends the signals to motors.
+ * DEFINE directionPins and motorPins
+ */
+  for(int i = 0; i < 4; i++){
+    if(motorSpeeds[i] < 0) {
+      digitalWrite(directionPins[i], LOW);
+    }else{
+      digitalWrite(directionPins[i], HIGH);
+    }
+    
+    if(abs(motorSpeeds[i])*2 < 255){
+      analogWrite(motorPins[i], abs(motorSpeeds[i])*2);
+    }else{
+      analogWrite(motorPins[i], 255);
+    }
+  }
+}
+
+void stopMotors(void) {
+  //stop the bot
+  for(int i = 0; i < 4; i ++) {
+    analogWrite(motorPins[i], 0);
+  }
+}
+
+void motorMath(void) {
+  //calculating drive speed and strafe speed
+  drive = (forward - PS4.getAnalogHat(LeftHatY)) - neutral;
+  strafe = PS4.getAnalogHat(LeftHatX) - neutral;
+  rotate = PS4.getAnalogHat(RightHatX) - neutral;
+  //calculating wheel speed
+  motorSpeeds[1] = drive + strafe - rotate;
+  motorSpeeds[0] = drive - strafe + rotate;
+  motorSpeeds[2] = drive - strafe - rotate;
+  motorSpeeds[3] = drive + strafe + rotate;
+}
+
+void driveMotorsWithPS4(void){
+  motorMath();
+  tellMotors();
+}
+
+//END-----------------------------------------------------------
+/*DEBUG Temp
+ *  if(DEBUG) {
+    Serial.print("------------------\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(); 
+    }
+    Serial.print("\n");
+    }
+ */
+
+

RobotCommands/RobotCommands3.2.ino

@@ -0,0 +1,454 @@
+/*
+ * RobotCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for RobotCommands ----------
+#include <Wire.h>
+#include <PS4BT.h>
+#include <usbhub.h>
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+#include <SPI.h>
+
+
+// Pin Out --------------------------------------------------
+const int pingPin = 30;
+const int motorPins[4] = {3,5,6,7};
+const int directionPins[4] = {22,24,26,28};
+
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 1; 
+const int neutral = 127;
+const int forward = 254;
+
+// Changing Variables ---------------------------------------
+//Robots
+unsigned char irReadings[8];
+int motorSpeeds[4];
+unsigned long cm;
+int drive = 0;
+int strafe = 0;
+int rotate = 0;
+int armCommand = 0;//changed to int from char set to 0 instead of 10
+
+//USB Shield
+
+bool printAngle, printTouch;
+uint8_t oldL2Value, oldR2Value;
+
+// Class Objects --------------------------------------------
+USB Usb;
+BTD Btd(&Usb);
+PS4BT PS4(&Btd, PAIR);
+// PS4BT PS4(&Btd);
+
+// Intial Setup ---------------------------------------------
+
+void setup() {
+  Wire.begin(); // join I2C bus
+  Serial.begin(115200);
+  #if !defined(__MIPSEL__)
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  #endif
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
+
+  for(int i = 0; i <4; i++) {
+    pinMode(motorPins[i], OUTPUT);
+    pinMode(directionPins[i], OUTPUT);
+  }
+ 
+}
+
+// Main Loop ------------------------------------------------
+
+void loop() {
+  Usb.Task(); 
+  if (PS4.connected()) { //PS4 button checks will be in this if() statement
+
+    //Motor Controls
+    if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 
+    || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117
+    || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117
+    || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
+
+      driveMotorsWithPS4();
+    } else {
+      stopMotors();
+    } //End of PS4.getAnaloghat if() statement 
+
+    //Arm Controls
+    /*CURRENT ISSUE 05-OCT-2017
+     * When set to getButtonPress causes arm to go crazy
+     * activates more than the intended servo cause unknown
+     * when the button is release arm returns to the position it was
+     * already in before pressing the button
+     * FROM LIBRARY NOTES
+     * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
+     * While getButtonClick(ButtonEnum b) will only return it once.
+     */
+    if(PS4.getButtonClick(TRIANGLE)|| PS4.getButtonPress(TRIANGLE)){//open the elbow
+      armCommand = 0;
+      tellArm();
+    } //End Triangle if() statement
+
+    if(PS4.getButtonClick(SQUARE)||PS4.getButtonPress(SQUARE)){//close the elbow
+      armCommand = 1;
+      tellArm();
+    }
+
+    if (PS4.getButtonClick(RIGHT)||PS4.getButtonPress(RIGHT)){//pivot arm right
+      armCommand = 2;
+      tellArm();
+    } //End Right if() statement
+
+    if (PS4.getButtonClick(LEFT)||PS4.getButtonPress(LEFT)){//pivot arm left
+      armCommand = 3;
+      tellArm();
+    } //End Left if() statement
+
+    if (PS4.getButtonClick(UP)|| PS4.getButtonPress(UP)){//move shoulder up
+      armCommand = 4;
+      tellArm();
+    } //End Up if() statement
+    
+    if (PS4.getButtonClick(DOWN)||PS4.getButtonPress(DOWN)){//move shoulder down
+      armCommand = 5;
+      tellArm();
+    }  //End Down if() statement
+
+    if(PS4.getButtonClick(L1)){//open gripper
+      armCommand = 6;
+      tellArm();
+    } //End L1 if() statement
+
+    if(PS4.getButtonClick(R1)){//close gripper
+      armCommand = 7;
+      tellArm();
+    } //End R1 if() statement
+
+    if(PS4.getButtonClick(CIRCLE)|| PS4.getButtonPress(CIRCLE)){//raise wrist
+      armCommand = 8;
+      tellArm();
+      }
+
+    if(PS4.getButtonClick(CROSS)||PS4.getButtonPress(CROSS)){//move wrist down
+      armCommand = 9;
+      tellArm();
+    } //End Cross if() statement
+
+    if(PS4.getButtonClick(OPTIONS)){//reset arm
+      armCommand = 10;
+      tellArm();
+    }
+
+    if(PS4.getButtonClick(SHARE)){//debug for arm
+      armCommand = 11;
+      tellArm();
+      askIR();
+      askPing();
+    }
+
+    
+    if (PS4.getButtonClick(TOUCHPAD)) {
+       autoMode();
+    }
+
+    if(PS4.getButtonClick(PS)|| PS4.getButtonPress(PS)){
+      armCommand = 12;
+      tellArm();
+    }
+
+//    if(PS4.getButtonClick(PS)){
+//      for(int j = 20; j < 45; j++){
+//        armCommand = j;
+//        tellArm();
+//        delay(500);
+//      }
+//    }
+    
+    //Sensor Controls
+    
+  }//End of PS4.connected if() statement
+}
+
+// Functions ------------------------------------------------
+// Sensor Functions
+void askIR(void){
+/*
+ * Used to update IR sensor readings.
+ * Sends I2C request to bus address #9. Asks for 16 bytes, only need 0,2,4,8,10,12,14
+ * Pass in an char array of 8 to be modified with new sensor readings, [0-255]
+ * #DEFINE irReadings
+ */
+  static unsigned char t, data[16];
+
+  t = 0;
+
+  Wire.requestFrom(9,16); // request 16 bytes from slave device #9
+  while (Wire.available()) { // slave may send less than requested
+    data[t] = Wire.read(); // receive a byte as character
+    t++;
+  }
+  for(char i = 0; i < 8; i++){
+    irReadings[i] = data[i*2];
+  }
+  
+  if(DEBUG) {
+    Serial.print("-----IR Readings-----\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(irReadings[i]);
+      Serial.print("~"); 
+    }
+    Serial.print("\n");
+  }
+}
+
+int askPing(void){
+/*
+ * Sends signal to activate Ping sensor and returns the distance in centimeters (may try using mm)
+ * Process takes 7-8 milliseconds.
+ * DEFINE pingPin
+ */
+  static unsigned long duration;
+
+  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
+  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
+  pinMode(pingPin, OUTPUT);
+  digitalWrite(pingPin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(pingPin, HIGH);
+  delayMicroseconds(5);
+  digitalWrite(pingPin, LOW);
+
+  // The same pin is used to read the signal from the PING))): a HIGH pulse
+  // whose duration is the time (in microseconds) from the sending of the ping
+  // to the reception of its echo off of an object.
+  pinMode(pingPin, INPUT);
+  duration = pulseIn(pingPin, HIGH);
+
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance of the object we
+  // take half of the distance traveled.
+  cm = duration / 29 / 2;
+  
+  if(DEBUG) {
+    Serial.print("-----Ping(cm)------\n");
+    Serial.print(cm);
+    Serial.print("\n");
+  }
+}
+
+//Arm Funcitons
+
+void tellArm(void){
+/*
+ * Should send the desired locations of the arm to the Arduino Uno over I2C at address #8
+ * Format of the data needs to be determined. Speed,Joint,Degree in a single signal or in 6 joint
+ * intervals with speeds being controlled by analog pin or something
+ */
+
+    Wire.beginTransmission(8); // transmit to device #8
+    Wire.write(armCommand);
+    Wire.endTransmission(); // stop transmitting\
+    
+    if(DEBUG) {
+      Serial.print("-----Arm Command----\n");
+      Serial.print(armCommand);
+      Serial.print("\n");
+    }
+}
+
+//Motor Functions
+
+void tellMotors(void){
+/*
+ * Uses the [(-)255-255] speed variables and sends the signals to motors.
+ * DEFINE directionPins and motorPins
+ */
+  for(int i = 0; i < 4; i++){
+    if(motorSpeeds[i] < 0) {
+      digitalWrite(directionPins[i], LOW);
+    }else{
+      digitalWrite(directionPins[i], HIGH);
+    }
+    
+    if(abs(motorSpeeds[i])*2 < 255){
+      analogWrite(motorPins[i], abs(motorSpeeds[i])*2);
+    }else{
+      analogWrite(motorPins[i], 255);
+    }
+  }
+}
+
+void driveStraight(int drive){
+/*
+ * Drives motors forwards or backwards(negative) at set speed
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+    if(drive < 0) {
+      digitalWrite(directionPins[i], LOW);
+    }else{
+      digitalWrite(directionPins[i], HIGH);
+    }
+    analogWrite(motorPins[i], abs(drive));
+  }
+}
+
+
+void turnRight(int drive,int degree){
+/*
+ * Drives motors so that the right motors are slower and the robot turns
+ * Degree is the percentage difference of left and right motors, Ex: If drive = 100 and degree 60, the right motors will run at 60 while left at 100
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+ 
+    digitalWrite(directionPins[i], HIGH);
+    
+    if(i == 1 || i == 2) {
+      analogWrite(motorPins[i], abs((drive*degree)/100));
+    }else{
+      analogWrite(motorPins[i], abs(drive));
+    }
+    
+  }
+}
+
+void turnLeft(int drive,int degree){
+/*
+ * Drives motors so that the left motors are slower and the robot turns
+ * Degree is the percentage difference of left and right motors, Ex: If drive = 100 and degree 60, the right motors will run at 60 while left at 100
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+ 
+    digitalWrite(directionPins[i], HIGH);
+    
+    if(i == 0 || i == 3) {
+      analogWrite(motorPins[i], abs((drive*degree)/100));
+    }else{
+      analogWrite(motorPins[i], abs(drive));
+    }
+    
+  }
+}
+
+void stopMotors(void) {
+  //stop the bot
+  for(int i = 0; i < 4; i ++) {
+    analogWrite(motorPins[i], 0);
+  }
+}
+
+void motorMath(void) {
+  //calculating drive speed and strafe speed
+  drive = (forward - PS4.getAnalogHat(LeftHatY)) - neutral;
+  strafe = PS4.getAnalogHat(LeftHatX) - neutral;
+  rotate = PS4.getAnalogHat(RightHatX) - neutral;
+  //calculating wheel speed
+  motorSpeeds[1] = drive + strafe - rotate;
+  motorSpeeds[0] = drive - strafe + rotate;
+  motorSpeeds[2] = drive - strafe - rotate;
+  motorSpeeds[3] = drive + strafe + rotate;
+}
+
+void driveMotorsWithPS4(void){
+  motorMath();
+  tellMotors();
+}
+
+//Autonomous Functions 
+
+void checkIR(void){
+/*
+ * Updates irReadings[i] with new values and checks to see if they fit our positioning conditions
+ * Flashes PS4 controller light Green when Good, Flash PS4 light Red when not 
+*/
+  askIR();
+  if((irReadings[3] > 150) && (irReadings[4] > 150)){
+    PS4.setLedFlash(10,10);
+    PS4.setLed(Green);
+    delay(1000);
+    PS4.setLedFlash(0,0);
+    PS4.setLedOff();
+  } else {
+    PS4.setLedFlash(10,10);
+    PS4.setLed(Red);
+    delay(1000);
+    PS4.setLedFlash(0,0);
+    PS4.setLedOff();
+  }
+}
+
+void autoMode(void){
+  cm = 50;
+  while(cm > 6){
+
+    if(PS4.getButtonClick(TOUCHPAD)){
+      stopMotors();
+      break;
+    }
+    askPing();
+    askIR();
+    if((irReadings[3] < 150) || (irReadings[4] < 150)){
+      driveStraight(100);
+      if(DEBUG) {
+        Serial.print("------Straight------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+      
+    } else if((irReadings[0] < 150) || (irReadings[1] < 150) || (irReadings[2] < 150)){
+      turnLeft(100,60);
+      if(DEBUG) {
+        Serial.print("------Left------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+    } else if((irReadings[5] < 150) || (irReadings[6] < 150) || (irReadings[7] < 150)){
+      turnRight(100,60);
+      if(DEBUG) {
+        Serial.print("------Right------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+    }
+  }
+  stopMotors();
+  armCommand = 6; //Open Gripper
+  tellArm();
+}
+
+
+
+//END-----------------------------------------------------------
+/*DEBUG Temp
+ *  if(DEBUG) {
+    Serial.print("------------------\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(); 
+    }
+    Serial.print("\n");
+    }
+ */
+
+

RobotCommands/RobotCommands3.3.ino

@@ -0,0 +1,458 @@
+/*
+ * RobotCommands.iso
+ *
+ *  Created on: Sep 30, 2017
+ *      Author: redsc_000
+ */
+
+// Including all Libraries used for RobotCommands ----------
+#include <Wire.h>
+#include <PS4BT.h>
+#include <usbhub.h>
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+#include <SPI.h>
+
+
+// Pin Out --------------------------------------------------
+const int pingPin = 30;
+const int motorPins[4] = {3,5,6,7};
+const int directionPins[4] = {22,24,26,28};
+
+
+// Constant Variables ---------------------------------------
+const bool DEBUG = 0; 
+const int neutral = 127;
+const int forward = 254;
+
+// Changing Variables ---------------------------------------
+//Robots
+unsigned char irReadings[8];
+int motorSpeeds[4];
+unsigned long cm;
+int drive = 0;
+int strafe = 0;
+int rotate = 0;
+int armCommand = 0;//changed to int from char set to 0 instead of 10
+
+//USB Shield
+
+bool printAngle, printTouch;
+uint8_t oldL2Value, oldR2Value;
+
+// Class Objects --------------------------------------------
+USB Usb;
+BTD Btd(&Usb);
+PS4BT PS4(&Btd, PAIR);
+// PS4BT PS4(&Btd);
+
+// Intial Setup ---------------------------------------------
+
+void setup() {
+  Wire.begin(); // join I2C bus
+  Serial.begin(115200);
+  #if !defined(__MIPSEL__)
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  #endif
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
+
+  for(int i = 0; i <4; i++) {
+    pinMode(motorPins[i], OUTPUT);
+    pinMode(directionPins[i], OUTPUT);
+  }
+ 
+}
+
+// Main Loop ------------------------------------------------
+
+void loop() {
+  Usb.Task(); 
+  if (PS4.connected()) { //PS4 button checks will be in this if() statement
+
+    //Motor Controls
+    if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 
+    || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117
+    || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117
+    || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
+
+      driveMotorsWithPS4();
+    } else {
+      stopMotors();
+    } //End of PS4.getAnaloghat if() statement 
+
+    //Arm Controls
+    /*CURRENT ISSUE 05-OCT-2017
+     * When set to getButtonPress causes arm to go crazy
+     * activates more than the intended servo cause unknown
+     * when the button is release arm returns to the position it was
+     * already in before pressing the button
+     * FROM LIBRARY NOTES
+     * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
+     * While getButtonClick(ButtonEnum b) will only return it once.
+     */
+    if(PS4.getButtonClick(TRIANGLE)|| PS4.getButtonPress(TRIANGLE)){//open the elbow
+      armCommand = 0;
+      tellArm();
+    } //End Triangle if() statement
+
+    if(PS4.getButtonClick(SQUARE)||PS4.getButtonPress(SQUARE)){//close the elbow
+      armCommand = 1;
+      tellArm();
+    }
+
+    if (PS4.getButtonClick(RIGHT)||PS4.getButtonPress(RIGHT)){//pivot arm right
+      armCommand = 2;
+      tellArm();
+    } //End Right if() statement
+
+    if (PS4.getButtonClick(LEFT)||PS4.getButtonPress(LEFT)){//pivot arm left
+      armCommand = 3;
+      tellArm();
+    } //End Left if() statement
+
+    if (PS4.getButtonClick(UP)|| PS4.getButtonPress(UP)){//move shoulder up
+      armCommand = 4;
+      tellArm();
+    } //End Up if() statement
+    
+    if (PS4.getButtonClick(DOWN)||PS4.getButtonPress(DOWN)){//move shoulder down
+      armCommand = 5;
+      tellArm();
+    }  //End Down if() statement
+
+    if(PS4.getButtonClick(L1)){//open gripper
+      armCommand = 6;
+      tellArm();
+    } //End L1 if() statement
+
+    if(PS4.getButtonClick(R1)){//close gripper
+      armCommand = 7;
+      tellArm();
+    } //End R1 if() statement
+
+    if(PS4.getButtonClick(CIRCLE)|| PS4.getButtonPress(CIRCLE)){//raise wrist
+      armCommand = 8;
+      tellArm();
+      }
+
+    if(PS4.getButtonClick(CROSS)||PS4.getButtonPress(CROSS)){//move wrist down
+      armCommand = 9;
+      tellArm();
+    } //End Cross if() statement
+
+    if(PS4.getButtonClick(OPTIONS) ||PS4.getButtonPress(OPTIONS)){//reset arm
+      armCommand = 13;
+      tellArm();
+    }
+
+    if(PS4.getButtonClick(SHARE)){//debug for arm
+      armCommand = 11;
+      tellArm();
+      askIR();
+      askPing();
+    }
+
+    
+    if (PS4.getButtonClick(TOUCHPAD)) {
+       autoMode();
+    }
+
+    if(PS4.getButtonClick(PS)|| PS4.getButtonPress(PS)){
+      armCommand = 12;
+      tellArm();
+    }
+
+//    if(PS4.getButtonClick(PS)){
+//      for(int j = 20; j < 45; j++){
+//        armCommand = j;
+//        tellArm();
+//        delay(500);
+//      }
+//    }
+    
+    //Sensor Controls
+    
+  }//End of PS4.connected if() statement
+}
+
+// Functions ------------------------------------------------
+// Sensor Functions
+void askIR(void){
+/*
+ * Used to update IR sensor readings.
+ * Sends I2C request to bus address #9. Asks for 16 bytes, only need 0,2,4,8,10,12,14
+ * Pass in an char array of 8 to be modified with new sensor readings, [0-255]
+ * #DEFINE irReadings
+ */
+  static unsigned char t, data[16];
+
+  t = 0;
+
+  Wire.requestFrom(9,16); // request 16 bytes from slave device #9
+  while (Wire.available()) { // slave may send less than requested
+    data[t] = Wire.read(); // receive a byte as character
+    t++;
+  }
+  for(char i = 0; i < 8; i++){
+    irReadings[i] = data[i*2];
+  }
+  
+  if(DEBUG) {
+    Serial.print("-----IR Readings-----\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(irReadings[i]);
+      Serial.print("~"); 
+    }
+    Serial.print("\n");
+  }
+}
+
+int askPing(void){
+/*
+ * Sends signal to activate Ping sensor and returns the distance in centimeters (may try using mm)
+ * Process takes 7-8 milliseconds.
+ * DEFINE pingPin
+ */
+  static unsigned long duration;
+
+  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
+  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
+  pinMode(pingPin, OUTPUT);
+  digitalWrite(pingPin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(pingPin, HIGH);
+  delayMicroseconds(5);
+  digitalWrite(pingPin, LOW);
+
+  // The same pin is used to read the signal from the PING))): a HIGH pulse
+  // whose duration is the time (in microseconds) from the sending of the ping
+  // to the reception of its echo off of an object.
+  pinMode(pingPin, INPUT);
+  duration = pulseIn(pingPin, HIGH);
+
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance of the object we
+  // take half of the distance traveled.
+  cm = duration / 29 / 2;
+  
+  if(DEBUG) {
+    Serial.print("-----Ping(cm)------\n");
+    Serial.print(cm);
+    Serial.print("\n");
+  }
+}
+
+//Arm Funcitons
+
+void tellArm(void){
+/*
+ * Should send the desired locations of the arm to the Arduino Uno over I2C at address #8
+ * Format of the data needs to be determined. Speed,Joint,Degree in a single signal or in 6 joint
+ * intervals with speeds being controlled by analog pin or something
+ */
+
+    Wire.beginTransmission(8); // transmit to device #8
+    Wire.write(armCommand);
+    Wire.endTransmission(); // stop transmitting\
+    
+    if(DEBUG) {
+      Serial.print("-----Arm Command----\n");
+      Serial.print(armCommand);
+      Serial.print("\n");
+    }
+}
+
+//Motor Functions
+
+void tellMotors(void){
+/*
+ * Uses the [(-)255-255] speed variables and sends the signals to motors.
+ * DEFINE directionPins and motorPins
+ */
+  for(int i = 0; i < 4; i++){
+    if(motorSpeeds[i] < 0) {
+      digitalWrite(directionPins[i], LOW);
+    }else{
+      digitalWrite(directionPins[i], HIGH);
+    }
+    
+    if(abs(motorSpeeds[i])*2 < 255){
+      analogWrite(motorPins[i], abs(motorSpeeds[i])*2);
+    }else{
+      analogWrite(motorPins[i], 255);
+    }
+  }
+}
+
+void driveStraight(int drive){
+/*
+ * Drives motors forwards or backwards(negative) at set speed
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+    if(drive < 0) {
+      digitalWrite(directionPins[i], LOW);
+    }else{
+      digitalWrite(directionPins[i], HIGH);
+    }
+    analogWrite(motorPins[i], abs(drive));
+  }
+}
+
+
+void turnRight(int drive,int degree){
+/*
+ * Drives motors so that the right motors are slower and the robot turns
+ * Degree is the percentage difference of left and right motors, Ex: If drive = 100 and degree 60, the right motors will run at 60 while left at 100
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+ 
+    digitalWrite(directionPins[i], HIGH);
+    
+    if(i == 1 || i == 2) {
+      analogWrite(motorPins[i], abs((drive*degree)/100));
+    }else{
+      analogWrite(motorPins[i], abs(drive));
+    }
+    
+  }
+}
+
+void turnLeft(int drive,int degree){
+/*
+ * Drives motors so that the left motors are slower and the robot turns
+ * Degree is the percentage difference of left and right motors, Ex: If drive = 100 and degree 60, the right motors will run at 60 while left at 100
+ * Using for autonoumous part 
+ */
+  for(int i = 0; i < 4; i ++) {
+ 
+    digitalWrite(directionPins[i], HIGH);
+    
+    if(i == 0 || i == 3) {
+      analogWrite(motorPins[i], abs((drive*degree)/100));
+    }else{
+      analogWrite(motorPins[i], abs(drive));
+    }
+    
+  }
+}
+
+void stopMotors(void) {
+  //stop the bot
+  for(int i = 0; i < 4; i ++) {
+    analogWrite(motorPins[i], 0);
+  }
+}
+
+void motorMath(void) {
+  //calculating drive speed and strafe speed
+  drive = (forward - PS4.getAnalogHat(LeftHatY)) - neutral;
+  strafe = PS4.getAnalogHat(LeftHatX) - neutral;
+  rotate = PS4.getAnalogHat(RightHatX) - neutral;
+  //calculating wheel speed
+  motorSpeeds[1] = drive + strafe - rotate;
+  motorSpeeds[0] = drive - strafe + rotate;
+  motorSpeeds[2] = drive - strafe - rotate;
+  motorSpeeds[3] = drive + strafe + rotate;
+}
+
+void driveMotorsWithPS4(void){
+  motorMath();
+  tellMotors();
+}
+
+//Autonomous Functions 
+
+void checkIR(void){
+/*
+ * Updates irReadings[i] with new values and checks to see if they fit our positioning conditions
+ * Flashes PS4 controller light Green when Good, Flash PS4 light Red when not 
+*/
+  askIR();
+  if((irReadings[3] > 150) && (irReadings[4] > 150)){
+    PS4.setLedFlash(10,10);
+    PS4.setLed(Green);
+    delay(1000);
+    PS4.setLedFlash(0,0);
+    PS4.setLedOff();
+  } else {
+    PS4.setLedFlash(10,10);
+    PS4.setLed(Red);
+    delay(1000);
+    PS4.setLedFlash(0,0);
+    PS4.setLedOff();
+  }
+}
+
+void autoMode(void){
+  cm = 50;
+  while(cm > 6){
+
+
+    askPing();
+    askIR();
+    if((irReadings[0] > 200) && (irReadings[1] > 200) && (irReadings[2] > 200)
+      && (irReadings[3] > 200) && (irReadings[4] > 200) && (irReadings[5] > 200)
+      && (irReadings[6] > 200) && (irReadings[7] > 200)){
+      stopMotors();
+      return;
+    }
+
+    if((irReadings[3] < 150) || (irReadings[4] < 150)){
+      driveStraight(100);
+      if(DEBUG) {
+        Serial.print("------Straight------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+      
+    } else if((irReadings[0] < 150) || (irReadings[1] < 150) || (irReadings[2] < 150)){
+      turnLeft(60,20);
+      if(DEBUG) {
+        Serial.print("------Left------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+    } else if((irReadings[5] < 150) || (irReadings[6] < 150) || (irReadings[7] < 150)){
+      turnRight(60,20);
+      if(DEBUG) {
+        Serial.print("------Right------\n");
+        for(char i = 0; i < 8; i++){
+          Serial.print(irReadings[i]); 
+          Serial.print("~");
+        }
+        Serial.print("\n");
+       }
+    }
+  }
+  stopMotors();
+  armCommand = 6; //Open Gripper
+  tellArm();
+}
+
+
+
+//END-----------------------------------------------------------
+/*DEBUG Temp
+ *  if(DEBUG) {
+    Serial.print("------------------\n");
+    for(char i = 0; i < 8; i++){
+      Serial.print(); 
+    }
+    Serial.print("\n");
+    }
+ */
+
+

RobotCommands/desktop.ini

@@ -0,0 +1,5 @@
+[.ShellClassInfo]
+InfoTip=This folder is shared online.
+IconFile=C:\Program Files\Google\Drive\googledrivesync.exe
+IconIndex=16
+