Uncommented mqtt funcs

Uncommented mqtt funcs

--- MQTTFuncs.ino
+++ MQTTFuncs.ino
@@ -19,10 +19,10 @@

   // powerOff
   if (payloadStr.equals(F("powerOff"))) {
-    //motionControlStop();
+    motionControlStop();
   // powerOn
   } else if (payloadStr.equals(F("powerOn"))) {
-  //motionControlStart();
+    motionControlStart();
   // getStatus
   } else if (payloadStr.equals(F("getStatus"))) {
       // snprintf (msg, 20, "PowerState: %i", curState);
@@ -45,9 +45,9 @@

   // powerReset
   } else if (payloadStr.equals(F("powerReset"))) {
-    //motionControlStop();
-    //delay(10000);
-    //motionControlStart();
+    motionControlStop();
+    delay(10000);
+    motionControlStart();
     loop();

   // resetESP
@@ -56,15 +56,15 @@
     delay(500);
     ESP.restart();
   } else if (payloadStr.equals(F("custom_action1"))) {
-    client.publish(TOPIC_T, "Custom action 1 run");
+    client.publish(TOPIC_T, "Custom action 1 unimplemented!");
   } else if (payloadStr.equals(F("custom_action2"))) {
-    client.publish(TOPIC_T, "Custom action 2 run");
+    client.publish(TOPIC_T, "Custom action 2 unimplemented!");
   } else if (payloadStr.equals(F("custom_action3"))) {
-    client.publish(TOPIC_T, "Custom action 3 run");
+    client.publish(TOPIC_T, "Custom action 3 unimplemented!");
   } else if (payloadStr.equals(F("custom_action4"))) {
-    client.publish(TOPIC_T, "Custom action 4 run");
+    client.publish(TOPIC_T, "Custom action 4 unimplemented!");
   } else if (payloadStr.equals(F("custom_action5"))) {
-    client.publish(TOPIC_T, "Custom action 5 run");
+    client.publish(TOPIC_T, "Custom action 5 unimplemented!");
   }
 }

@@ -88,4 +88,4 @@
       delay(5000);
     }
   }
-}+}


Turn off all needles.

Turn off all needles.

--- brotherKH930_arduino.ino
+++ brotherKH930_arduino.ino
@@ -12,6 +12,9 @@
   Serial.begin(115200);
   brother.start();
   Serial.println("Ready.");
+
+  for (int i=0; i<200; i++)
+    brother.needle(0, false);
 }

 boolean changed = true;


Update Robot_Code.ino

Update Robot_Code.ino

Added comments to movement code section to better explain what was done

--- Robot_Code.ino
+++ Robot_Code.ino
@@ -1,1347 +1,1365 @@
-//#include <Array.h>
-
-#include <SoftwareSerial.h>
-#include <SPI.h>
-#include <Pixy.h>
-#include <NewPing.h>
-#include <Math.h>
-#include <Time.h>
-#include <SD.h>
-
-#define X_CENTER 160L
-
-int wheel_pwm[] = {2,3,4}; //pins on motor drive to control speed
-int wheel_direction[] = {5,6,7}; //pins to control wheel direction
-double wheel_angles[] = {270, 30, 150}; //wheel angles are defined
-double wheel_speed[3]; //speed of each wheel is stored in this array
-double max_speed = 70; //maximum speed
-double max_speedR = 40; //rotate speed
-
-//create a new instance of Pixy
-Pixy pixy;
-
-double cm[2]; //sensed distance of sonar sensor : 2 represents array(2 sonar sensors)
-
-int trigger[6] = {30, 32, 46, 36, 38, 40}; //trigger pins assignment  (sends)
-int echo[6] = {31, 33, 47, 37, 39, 41}; //echo pins (receives)
-
-int colliDistance_cm = 25; //distances to define collision in cm/in
-int colliDistance_in = 12;
-
-//set up the sonar sensors for NewPing Library
-NewPing sonar[6] =
-{
-  NewPing(trigger[0], echo[0], 500), //500 represents type of sensor
-  NewPing(trigger[1], echo[1], 500),
-  NewPing(trigger[2], echo[2], 500),
-  NewPing(trigger[3], echo[3], 500),
-  NewPing(trigger[4], echo[4], 500),
-  NewPing(trigger[5], echo[5], 500),
-};
-
-int IR_proximity_pins[] = {A0, A1, A2, A3}; // pins for the proximity detection
-int IR_proximity_state[4];              // state of IR pins
-
-double solar[9]; //solar vectors are stored here
-double solar_angles[] = {290, 330, 10, 50, 90, 130, 170, 210, 250}; //solar angles defined
-int solar_pins[] = {A5, A7, A8, A9, A10, A12, A13, A14, A15}; // solar pins
-//analog pins assigned to solar panels
-
-boolean light1_state = false;
-boolean light2_state = false;
-boolean water_state = false;
-boolean CO2_state = false;
-
-const int motor_relay = 8;   //does not want relay to change.
-
-void setup() {     //does not return anything
-  Serial.begin(9600);  // begin serial to computer
-
-  for (int i = 0; i < 3; i++) {
-    pinMode(wheel_direction[i], OUTPUT); //wheel directions are OUTPUT
-  }
-  for (int i = 0; i < 4; i++) { //i++ for adding
-    pinMode(IR_proximity_pins[i], INPUT);
-  }
-  pinMode(motor_relay, OUTPUT);
-  digitalWrite(motor_relay, HIGH);
-
-  Serial3.begin(9600);    // begin serial to BASE ARDUINO
-  Serial1.begin(9600);    // begin serial to SD card
-
-  pixy.init();  // initialize pixy camera library functions
-  delay(100);  //delay of .1 seconds so that you do not compile errors
-  // find_red2();
-  // setTime(7,0,0,1,1,1);    // (hr,min,sec,day,month,yr)
-
-}
-/*
-  #include <SoftwareSerial.h>
-
-  SoftwareSerial mySerial(0, 1); // RX, TX
-
-  void setup()
-  {
-  // Open serial communications and wait for port to open:
-  Serial.begin(57600);
-  while (!Serial) {
-    ; // wait for serial port to connect. Needed for Leonardo only
-  }
-
-
-  Serial.println("Goodnight moon!");
-
-  // set the data rate for the SoftwareSerial port
-  mySerial.begin(4800);
-  mySerial.println("Hello, world?");
-  }
-
-  void loop() // run over and over
-  {
-  if (mySerial.available())
-     Serial.write(mySerial.read());
-  if (Serial.available())
-     mySerial.write(Serial.read());
-  }
-*/
-void loop() {
- // motospd(5,5,5);
-//Pixy_Align();
-//updateSonar();
-//sonarInfoDisplay();
-//Do_Thingy();
-//Go_To_Object();
-wheel_speeds(40,270);
-//motospd(0,0,40);
-  delay(1000);
-}
-void Do_Thingy(){
-  boolean aligned = false;
-  Serial.print(Am_I_Close());
-  if(!aligned){
-    Pixy_Align();
-    aligned = true;
-  }
-  else if (!Am_I_Close()){
-    Go_To_Object();
-  }
-  else{
-    return;
-  }
-}
-void Go_To_Object(){
-  if(Am_I_Close()){
-    motospd(0,0,0);
-  }
-  else{
-    motospd(0,40,-40);
-  }
-  delay(100);
-
-}
-boolean Am_I_Close(){
-   unsigned int uS = sonar[2].ping();
-    cm[1] = sonar[1].convert_cm(uS);
-    cm[2] = sonar[2].convert_cm(uS);
-    Serial.print(cm[1]);
-    Serial.print(cm[2]);
-    if (cm[1] <=30 && cm[1] != 0 || cm[2] <= 30 && cm[2] != 0){
-      return true;
-    }
-    else{
-      return false;
-    }
-
-}
-void Pixy_Align()
-{
-  Serial.println("-------------------------------------------------");
-  Serial.println("-------------------------------------------------");
-  get_coordinates();
-  int rotation_update_delay = 10;
-  int tolerance = 15;
-  int blocks = pixy.getBlocks(17);
-  if (pixy.blocks[0].x < (160 - tolerance) )
-  {
-    Serial.println("Rotating CoutnerClockwise...");
-    motospd(20, 20, 20);
-    delay(rotation_update_delay);
-    Pixy_Align();
-  }
-  else if (pixy.blocks[0].x > (160 + tolerance) )
-  {
-    Serial.println("Rotating Clockwise...");
-    motospd(-20, -20, -20);
-    delay(rotation_update_delay);
-    Pixy_Align();
-  }
-  else {
-//    Pixy_finetune();
- Serial.println("pixy has been aligned");
-    motospd(0, 0, 0);
-    return;
-  }
-
-
-}
-
-void Pixy_finetune() {
-
-  Serial.println("------------||FINE TUNE IN PROGRESS||-----------");
-  Serial.println("-------------------------------------------------");
-  get_coordinates();
-  int rotation_update_delay = 10;
-  int tolerance = 5;
-  int spd = 10;
-  int blocks = pixy.getBlocks(17);
-  if (pixy.blocks[0].x < (160 - tolerance) )
-  {
-    Serial.println("Rotating CoutnerClockwise...");
-    motospd(spd, spd, spd);
-    delay(rotation_update_delay);
-    Pixy_finetune();
-  }
-  else if (pixy.blocks[0].x > (160 + tolerance) )
-  {
-    Serial.println("Rotating Clockwise...");
-    motospd(-1 * spd, -1 * spd, -1 * spd);
-    delay(rotation_update_delay);
-    Pixy_finetune();
-  }
-  else {
-    Serial.println("pixy has been aligned");
-    motospd(0, 0, 0);
-    return;
-  }
-
-}
-
-void get_coordinates()
-{
-
-  int blocks = pixy.getBlocks(17);
-  for (int k = 0; k < blocks; k++) {
-    if (pixy.blocks[k].signature == 1) {
-      Serial.print("The X coordinate is:");
-      Serial.print(pixy.blocks[k].x);
-      Serial.println();
-      Serial.print("The Y coordinate is:");
-      Serial.print(pixy.blocks[k].y);
-      Serial.println();
-
-    }
-  }
-
-}
-void wheel_speeds(float tot_spd, float ang){
-  float results[4];
-  Relative_ang(results,ang);
-
-  float Motor_2 = abs(((((-1/sqrt(3))*tan((results[0]/180)*M_PI))-1)*(tot_spd/(1/cos((results[0]/180)*M_PI)))));
-  float Motor_1 = abs(((((1/sqrt(3))*tan((results[0]/180)*M_PI))-1)*(tot_spd/(1/cos((results[0]/180)*M_PI)))));
-
-  float Motor_Comp = .0018*pow(results[0],3) - 0.1019*pow(results[0],2) + 2.446*results[0] + 0.8571;
-
-int a = abs(results[1])-1;
-int b = abs(results[2])-1;
-int c = abs(results[3])-1;
-
- float motors[3];
- motors[a]= (results[1]/(abs(results[1]))) * Motor_1;
- motors[b]= (results[2]/(abs(results[2]))) * Motor_2;
- motors[c]= (results[3]/(abs(results[3]))) * Motor_Comp;
-
-Serial.println(results[0]);
-Serial.println(results[1]);
-Serial.println(results[2]);
-Serial.println(results[3]);
-Serial.println("------------------------------------");
-Serial.println("--------------Motors---------------");
-Serial.println("------------------------------------");
-Serial.println(motors[0]);
-Serial.println(motors[1]);
-Serial.println(motors[2]);
-Serial.println("------------------------------------");
-Serial.println("--------------Results---------------");
-Serial.println("------------------------------------");
-
- motospd(motors[0],motors[1],motors[2]); //At Min Speed, set compensator to 18.
-}
-
-
-void Relative_ang(float *ptr, float ang){
-//float a,b,c,d;
-// float new_config[4]={0,0,0,0};
-// float *ptr[4];
- float new_ang;
-
-
-    if(ang >= 0 && ang<= 30){
-      new_ang = ang;
-     //new_config = {new_ang, 3,2,1};
-      ptr[0] = new_ang;
-      ptr[1] = -3;
-      ptr[2] = 2;
-      ptr[3] = -1;
-
-    }
-     else if(ang > 330 && ang<= 360){
-      new_ang = ang;
-     //new_config = {new_ang, 3,2,1};
-      ptr[0] = new_ang - 360;
-      ptr[1] = -3;
-      ptr[2] = 2;
-      ptr[3] = 1;
-
-    }
-    else if (ang > 30 && ang <= 90){
-     new_ang = ang - 60;
-    //new_config = {new_ang, -2,-1,3};
-     ptr[0] = new_ang;
-      ptr[1] = 2;
-      ptr[2] = -1;
-      ptr[3] = -1*(new_ang/abs(new_ang))*3;
-    }
-    else if (ang > 90 && ang <= 150){
-      new_ang = ang - 120;
-     //new_config = {new_ang, 3,1,2};
-      ptr[0] = new_ang;
-      ptr[1] = 3;
-      ptr[2] = -1;
-      ptr[3] = -1*(new_ang/abs(new_ang))*2;
-    }
-    else if(ang > 150 && ang<= 210){
-      new_ang = ang - 180;
-     // new_config = {new_ang, -3,-2,1};
-      ptr[0] = new_ang;
-      ptr[1] = 3;
-      ptr[2] = -2;
-      ptr[3] = -1*(new_ang/abs(new_ang))*1;
-    }
-    else if(ang > 210 && ang<= 270){
-      new_ang = ang - 240;
-     // new_config = {new_ang, 2,1,3};
-      ptr[0] = new_ang;
-      ptr[1] = -2;
-      ptr[2] = 1;
-      ptr[3] = -1*(new_ang/abs(new_ang))*3;
-    }
-    else if(ang > 270 && ang<= 330){
-      new_ang = ang - 300;
-     // new_config = {new_ang, -3,-1,2};
-      ptr[0] = new_ang;
-      ptr[1] = -3;
-      ptr[2] = 1;
-      ptr[3] = -1*(new_ang/abs(new_ang))*2;
-     }
-
-
- return 0;
-
-
-}
-
-//float Relative_ang(float ang){
-// //float a,b,c,d;
-// float new_config[4]={0,0,0,0};
-// float *ptr[4];
-//  float new_ang;
-//
-//
-//     if((ang > 330 && ang<= 360) || (ang >= 0 && ang<= 30)){
-//       new_ang = ang;
-//      new_config = {new_ang, 3,2,1};
-//        a= new_ang;
-//
-//
-//     }
-//     else if (ang > 30 && ang <= 90){
-//      new_ang = ang + 300;
-//     new_config = {new_ang, -2,-1,3};
-//     }
-//     else if (ang > 90 && ang <= 150){
-//       new_ang = ang + 240;
-//      new_config = {new_ang, 3,1,2};
-//     }
-//     else if(ang > 150 && ang<= 210){
-//       new_ang = ang + 180;
-//      new_config = {new_ang, -3,-2,1};
-//     }
-//     else if(ang > 210 && ang<= 270){
-//       new_ang = ang + 120;
-//      new_config = {new_ang, 2,1,3};
-//     }
-//     else if(ang > 270 && ang<= 330){
-//       new_ang = ang + 60;
-//      new_config = {new_ang, -3,-1,2};
-//      }
-//
-//  for(int i = 0; i < 4; i++){
-//    ptr[i] = &new_config[i]; // assign the address of integer.
-//  }
-//  return **ptr;
-//
-//
-//}
-
-
-
-void Lets_Move(double ang, int tot_spd, int comp){
-float Motor_2;
-float Motor_1;
-
-  Motor_2 = ((((-1/sqrt(3))*tan((ang/180)*M_PI))-1)*(tot_spd/(1/cos((ang/180)*M_PI))));
-  Motor_1 = -1*((((1/sqrt(3))*tan((ang/180)*M_PI))-1)*(tot_spd/(1/cos((ang/180)*M_PI))));
-  Serial.println("-----------------------");
-  Serial.println("-----------------------");
-  Serial.print("Motor 1 Speed:");
-  Serial.println(Motor_1);
-  Serial.print("Motor 2 Speed:");
-  Serial.println(Motor_2);
-
- motospd(comp,Motor_1,Motor_2); //At Min Speed, set compensator to 18.
- delay(1000);
-}
-
-// Just a mini test cycle to try and visualize each station without having to wait
-// the actual time it takes for lighting or getting CO2 or whatever its doing
-void test_cycle() {
-  delay(2000);   //waits 2 seconds
-  getWater(1);   //gets water for a minute
-  delay(1000);   //waits 10 seconds
-  Serial.println("preparing to measure moisture");
-  Serial1.write('M');
-  delay(1000);   //waits 1 second
-  delay(1000);                  //turn on lights
-  Serial3.println("7H2");
-  delay(3000); //waits 3 seconds
-  light1_state = true;
-  getSun();
-  Serial3.println("7H1");
-  delay(120000);
-  Serial3.println("7H2");
-  delay(3000);
-  light1_state = true;
-  getSun();
-  Serial3.println("7H1");
-  delay(120000);
-  getCO2(1);
-
-}
-
-// cycle for the entire day
-void day_cycle() {
-  getWater(1);
-  delay(500);
-  Serial.println("preparing to measure moisture");
-  Serial1.write('M');
-
-  // measure after getting water, then again 2 full cycles later
-  for (int i = 0; i < 7; i++) {
-    delay(100);
-    mini_cycle();
-    delay(100);
-    if (i == 2) {
-      Serial.println("preparing to measure moisture");
-      Serial1.write('M');
-    }
-    delay(100);
-  }
-}
-
-//run the full 5 light cycle then get CO2
-void mini_cycle() {
-  light_cycle();
-  delay(2000);
-  getCO2(30);
-  delay(500);
-}
-
-// single light cycle just gets light once then sits
-void light_cycle() { //cycling light
-  for (int i = 0; i < 5; i++) {
-    light_loop();
-    delay(1800000);       //no sun for 30 min
-    motospd(30, 30, 30);  // after getting in a lit position
-    delay(1100);          // rotate a little after each cycle
-    motospd(0, 0, 0);     // to try and be more distributed
-  }
-}
-
-void light_loop() {
-  delay(50);                  //turn on lights
-  Serial3.println("7H3");
-  delay(3000);
-  light1_state = true;   //light is on now because it is true
-  getSun();
-  Serial3.println("7H5");
-  delay(300000);          //get sun for 5 min
-}
-
-void getWater(int water_time) { //time to get water in minutes
-  find_red1();   //indicator for detecting
-  delay(1000);
-  String msg = "5H";
-  msg = msg + water_time;
-  Serial3.println(msg);
-  water_state = true;
-  delay(water_time * 30000);
-  Serial3.println("5L");
-  delay(2000);
-  water_state = false;
-  delay(2000);
-  backAwayFromRed();
-}
-
-void getCO2(int CO2_time) {   // time to sit in CO2 cage in minutes
-  find_green1();
-  delay(1000);
-  String msg = "4H";
-  msg = msg + CO2_time;
-  Serial3.println(msg);  //writes to arduino
-  CO2_state = true;
-  delay(60000);
-  delay(CO2_time * 60000);
-  delay(2000);
-  delay(60000);
-  CO2_state = false;
-  backAwayFromGreen();
-}
-
-void find_red1() {   // parent function to call recursive functions and talk to BASE
-  // turn on the lights temporarily for tracking
-  Serial.println("Searching for red");
-  delay(1000);
-  Serial3.println("6H3");
-  delay(50);
-  light2_state = true;
-
-  digitalWrite(motor_relay, LOW); //turn motors on
-  delay(250);
-
-  find_red2();       // locate, center, and approach target
-  horizontalCenter();// horiztonally center along the wall
-  adjustRed();       // adjust distance to wall to line up with water faucet
-
-  digitalWrite(motor_relay, HIGH); //turn motors off
-  delay(100);
-  Serial3.println("6L");
-  delay(1000);
-}
-
-void find_red2() {
-  motospd(20, 20, 20);    // spin until red is detected
-  locate_red();
-  Serial.println("red detected");
-  /*
-    centerObject2(1);    // center with target
-
-    delay(50);
-
-    static int i = 0;  //only can be changed outside the global scope; can't be changed in a for loop
-    int j;
-    uint16_t blocks;   //allocated integer of 16 bits
-    char buf[32];    //character
-    int32_t xError;
-    blocks = pixy.getBlocks(17);
-
-    int height = pixy.blocks[0].height;;
-    Serial.print("height:  ");
-    Serial.println(height); //prints the height of the variable height
-
-    double dist_away = 1866.0 / height;    // use height to get distance
-    //distance away in inches
-
-    if (dist_away > 100 || dist_away < 0) {      // throw out error when pixy picks up
-     motospd(max_speedR,max_speedR,max_speedR); // small background objects
-     delay(500);
-     motospd(0,0,0);  //not moving
-     find_red2();
-     return;
-    }
-    Serial.print("distance away:  ");
-    Serial.println(dist_away);
-
-    pixyRun2(dist_away, 1);    //approach target
-    delay(10);*/
-}
-
-void locate_red() {    //spin and refresh camera until red is detected
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-  int32_t xError;
-  blocks = pixy.getBlocks(17);
-
-  for (int k = 0; k < blocks; k++) {
-    Serial.println("for loop started");
-    if (pixy.blocks[k].signature == 1) {
-      Serial.println("found block");
-      delay(500);
-      motospd(0, 0, 0);
-      delay(5000);
-      return;
-    }
-  }
-  delay(50);
-  locate_red();
-}
-
-void backAwayFromRed() {     // back away from red wall before performing next task
-  digitalWrite(motor_relay, LOW); //low means on in this case
-  delay(250);
-
-  motospd(0, -max_speed, max_speed);
-  delay(2000);
-  motospd(0, 0, 0);
-
-  digitalWrite(motor_relay, HIGH);
-}
-
-void pixyAlignRed() {     //align straight with the red target
-  Serial.println("Aligning with object");
-  updateSonar();
-  delay(100);
-  updateSonar();
-  sonarInfoDisplay();
-
-  if (abs(cm[0] - cm[1]) < 2) { //spin left or right based on sonar values
-    Serial.println("Aligned");
-    return;
-  } else if (cm[0] > cm[1]) {
-    motospd(40, 10, 10);
-    delay(250);
-    motospd(0, 0, 0);
-    delay(100);
-    pixyAlignRed();
-  } else {
-    motospd(-40, -10, -10);
-    delay(250);
-    motospd(0, 0, 0);
-    delay(100);
-    pixyAlignRed();
-  }
-}
-
-void adjustRed() {   // adjust distance from red to line up with water faucet
-  Serial.println("adjusting");
-  updateSonar();
-  delay(100);
-  updateSonar();
-
-  double adjust = 24 - ((cm[0] + cm[1]) / 2.0);
-  Serial.print("adjust distance: ");
-  Serial.println(adjust);
-
-  if (adjust > 0) {  // move forward or backward based on sonar
-    motospd(0, -max_speed, max_speed);
-    double adjustTime = adjust / 2.54 / 11 * 1000;
-    delay(adjustTime);
-    motospd(0, 0, 0);
-  } else {
-    adjust = -adjust;
-    motospd(0, max_speed, -max_speed);
-    double adjustTime = adjust / 2.54 / 11 * 1000;
-    delay(adjustTime);
-    motospd(0, 0, 0);
-  }
-}
-
-void horizontalCenter() {   // center left or right along the wall to line up with water
-  Serial.println("horizontally center/align to object");
-
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-  int32_t xError;
-  blocks = pixy.getBlocks(17);
-
-  int center = pixy.blocks[0].x;    // use pixy x-axis center function to line it up
-  Serial.print("center: ");
-  Serial.println(center);
-
-  if (abs(center - 150) < 5) {
-    Serial.println("hor. centered with the object - ready for dist. adjust");
-    return;
-  } else if (center < 150) {
-    Serial.println("hor. center - moving left");
-    for (int i = 0; i < 3; i++) {
-      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 90);
-    }
-    motorWrite();
-    delay(200);
-    motospd(0, 0, 0);
-    delay(1000);
-    horizontalCenter();
-  } else {
-    Serial.println("hor. center - moving right");
-    for (int i = 0; i < 3; i++) {
-      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 270);
-    }
-    motorWrite();
-    delay(200);
-    motospd(0, 0, 0);
-    delay(1000);
-    horizontalCenter();
-  }
-
-}
-
-void find_green1() {   // parent function to call recursive functions and talk to BASE
-  delay(1500);           // turn on lights temporarily for color tracking
-  Serial3.println("6H5");
-  delay(1000);
-  light2_state = true;
-
-  digitalWrite(motor_relay, LOW);
-  delay(250);
-
-  find_green2();       // locate, center, and approach green
-  horizontalCenterGreen();   // hor line up to object before adjust
-  adjustGreen();       // adjust distance to green to line up with CO2 cage
-
-  digitalWrite(motor_relay, HIGH);
-  delay(100);
-  Serial3.println("6L");
-  delay(1000);
-}
-
-//recursive function to narrow in on the green
-void find_green2() {
-  motospd(-max_speedR, -max_speedR, -max_speedR);    //spin until green is detected
-  delay(300);
-  locate_green();
-  Serial.println("green detected");
-
-
-  centerObject2(3);    // center with green
-
-  delay(50);
-
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-  int32_t xError;
-  blocks = pixy.getBlocks(17);
-
-  int height;
-  //height = pixy.blocks[0].height;
-
-  if (blocks == 1) {
-    height = pixy.blocks[0].height;
-  } else {
-    height = pixy.blocks[1].height;
-  }
-
-  Serial.print("height:  ");
-  Serial.println(height);
-
-  double dist_away = 1800.0 / height;    // use height to find distance away
-  //distance away in inches
-
-  if (dist_away > 100 || dist_away < 0) {  //get rid of background noise detection errors
-    find_green2();
-    return;
-  }
-  Serial.print("distance away:  ");
-  Serial.println(dist_away);
-
-  pixyRun2(dist_away, 3);    // approach target
-  delay(10);
-}
-
-void locate_green() {  //spin and refresh camera until green is detected
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-  int32_t xError;
-  blocks = pixy.getBlocks(17);
-
-  for (int k = 0; k < blocks; k++) {
-    if (pixy.blocks[k].signature == 3) {
-      delay(300);
-      motospd(0, 0, 0);
-      return;
-    }
-  }
-  delay(50);
-  locate_green();
-}
-
-void centerObject2(int color_num) {    // recursive center with target
-
-  Serial.println("centering");
-  delay(50);
-
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-
-  blocks = pixy.getBlocks(17);
-
-  int block_num;
-  int error;  //error if you do not input 1 or 0
-  if (blocks) {
-
-    if (color_num == 1) {
-      block_num = 0;
-    } else {
-      if (blocks == 1) {
-        block_num = 0;
-      } else {
-        block_num = 1;
-      }
-    }
-
-    int height = pixy.blocks[block_num].height;
-    if (height < 10) {
-      if (color_num == 1) {
-        find_red2();
-      } else {
-        find_green2();
-      }
-      return;
-    }
-
-    uint32_t center = pixy.blocks[block_num].x;    // use x-axis center function to align
-    error = X_CENTER - center;                     // robot with target
-    Serial.print("center: ");
-    Serial.print(center);
-    Serial.print("   error: ");
-    Serial.println(error);
-  }
-
-  if (abs(error) < 10) {
-    Serial.println("centered");    // <10 is good enough alignment with margin of error
-    return;
-  }
-
-  int spin_time = abs(error) * 4.5;    // spin time based on size of center error
-  Serial.print("Spin Time:  ");
-  Serial.println(spin_time);
-
-  if (error >= 0) {
-    motospd(max_speedR, max_speedR, max_speedR);
-  } else {
-    motospd(-max_speedR, -max_speedR, -max_speedR);
-  }
-  delay(spin_time);
-  motospd(0, 0, 0);
-
-  centerObject2(color_num);    //recursive call to improve accuracy
-}
-
-void pixyRun2(double dist_away, int color_num) {   //approach target
-  if (anyCollision(5)) {  // if theres a collision, move away
-    getAwayFromWall();
-    if (color_num == 1) {
-      find_red2();
-      return;
-    } else if (color_num == 3) {
-      find_green2();
-      return;
-    }
-  }
-  if (color_num == 1) {    // distance for red is 15 in
-    if (dist_away < 15) {
-      pixyAlignRed();
-      return;
-    }
-  } else if (color_num == 3) { //distance for green is 20 in
-    if (dist_away < 20) {
-      pixyAlignGreen();
-      return;
-    }
-  }
-
-  double approach_time = dist_away / 11 * 1000.0;  // approach time based on distance
-  motospd(0, max_speed, -max_speed);
-  if (color_num == 1) {
-    delay(approach_time / 3.0);    // move part of the way
-  } else if (color_num == 3) {
-    delay(approach_time / 3.0);
-  }
-  motospd(0, 0, 0);
-  delay(10);
-
-  if (color_num == 3) {  //recursive call to continue process
-    delay(50);
-    find_green2();
-  } else if (color_num == 1) {
-    delay(50);
-    find_red2();
-  }
-}
-
-void backAwayFromGreen() {
-  digitalWrite(motor_relay, LOW);
-  delay(250);
-
-  motospd(0, -max_speed, max_speed);
-  delay(4500);
-  motospd(0, 0, 0);
-
-  digitalWrite(motor_relay, HIGH);
-}
-
-void pixyAlignGreen() { //align robot with green to line up with CO2 cage
-  Serial.println("Aligning with object");
-  updateSonar();
-  sonarInfoDisplay();
-
-  if (abs(cm[0] - cm[1]) < 3) { // move left or right based on sonar
-    return;
-  } else if (cm[0] > cm[1]) {
-    motospd(40, 10, 10);
-    delay(300);
-    motospd(0, 0, 0);
-    delay(100);
-    pixyAlignGreen();
-  } else {
-    motospd(-40, -10, -10);
-    delay(300);
-    motospd(0, 0, 0);
-    delay(100);
-    pixyAlignGreen();
-  }
-}
-
-void horizontalCenterGreen() {   // center left/right along the wall to line up with water
-  Serial.println("horizontally center/align to object");
-
-  static int i = 0;
-  int j;
-  uint16_t blocks;
-  char buf[32];
-  int32_t xError;
-  blocks = pixy.getBlocks(17);
-
-  int center = pixy.blocks[0].x;    // use pixy x-axis center function to line it up
-  Serial.print("center: ");
-  Serial.println(center);
-
-  if (abs(center - 150) < 5) {
-    Serial.println("hor. centered with the object - ready for dist. adjust");
-    return;
-  } else if (center < 150) {
-    Serial.println("hor. center - moving left");
-    for (int i = 0; i < 3; i++) {
-      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 90);
-    }
-    motorWrite();
-    delay(200);
-    motospd(0, 0, 0);
-    delay(1000);
-    horizontalCenterGreen();
-  } else {
-    Serial.println("hor. center - moving right");
-    for (int i = 0; i < 3; i++) {
-      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 270);
-    }
-    motorWrite();
-    delay(200);
-    motospd(0, 0, 0);
-    delay(1000);
-    horizontalCenterGreen();
-  }
-}
-
-
-void adjustGreen() {   // adjust distance to green to line up with CO2 cage
-  updateSonar();
-
-  double adjust = 48 - ((cm[0] + cm[1]) / 2.0);
-  if (adjust > 0) {  // move forward or backward based on sonar
-    motospd(0, -max_speed, max_speed);
-    double adjustTime = adjust / 2.54 / 11 * 1000;
-    delay(adjustTime);
-    motospd(0, 0, 0);
-  } else {
-    adjust = -adjust;
-    motospd(0, max_speed, -max_speed);
-    double adjustTime = adjust / 2.54 / 11 * 1000;
-    delay(adjustTime);
-    motospd(0, 0, 0);
-  }
-}
-
-void getSun() {  // find light
-  digitalWrite(motor_relay, LOW); //turn on the motor
-
-  updateSolar();
-  delay(100);
-  solarInfoDisplay();
-
-  if (evenSun()) {  // if its evenly distributed in the sun then its done
-    digitalWrite(motor_relay, HIGH);
-    return;
-  }
-
-  double solar_angle = getSolarAngle();  //get angle of light
-  Serial.print("Solar Angle:  ");
-  Serial.println(solar_angle);
-
-  delay(2000);
-
-  rotateToLight2(solar_angle);  //rotate "front" of robot towards that solar angle
-
-  solarRun(solar_angle);    // approach light
-
-}
-
-//calculate the solar angle in degree by
-//using all 3 solar vectors the outcome
-//ranges from 0degree to 360degree
-double getSolarAngle() {
-  double x = 0;
-  double y = 0;
-  for (int i = 0; i < 9; i++) {
-    x += solar[i] * newCos(solar_angles[i]);
-    y += solar[i] * newSin(solar_angles[i]);
-  }
-  return newAtan(x, y);
-}
-
-// see if all the solar sensors are within range of each other
-// to see if its evenly distributed
-boolean evenSun() {
-  for (int i = 0; i < 9; i++) {
-    for (int j = i + 1; j < 9; j++) {
-      if (abs(solar[i] - solar[j]) > 250) {
-        return false;
-      }
-    }
-  }
-  return true;
-}
-
-void rotateToLight(double solar_angle) {   //rotate towards the given angle
-
-  double rotationAngle = solar_angle - 90;
-  if (rotationAngle < 0) {
-    rotationAngle += 360;
-  }
-
-  double spinTime = rotationAngle * 13000 / 360;
-  Serial.print("spin time :");
-  Serial.println(spinTime);
-
-  motospd(max_speedR, max_speedR, max_speedR);
-  delay(spinTime);
-  motospd(0, 0, 0);
-
-  if (anyCollision(25)) {
-    motospd(0, -max_speed, max_speed);
-    delay(500);
-    motospd(0, 0, 0);
-  }
-  delay(50);
-}
-
-void rotateToLight2(double solar_angle) {   // rotate to the solar angle
-  if (solar_angle >= 90 && solar_angle <= 270) {  // if its in this half of the circle,
-    double rotationAngle = solar_angle - 90;      // then rotate CCW
-
-    double spinTime = rotationAngle * 13000 / 360;
-    Serial.print("spin time :");
-    Serial.println(spinTime);
-
-    motospd(max_speedR, max_speedR, max_speedR);
-    delay(spinTime);
-    motospd(0, 0, 0);
-  } else {               // if its in the other half of the circle, then rotate CW
-    double rotationAngle;
-    if (solar_angle < 90) {
-      rotationAngle = 90 - solar_angle;
-    } else {
-      rotationAngle = 90 + (360 - solar_angle);
-    }
-    double spinTime = rotationAngle * 13000 / 360;
-    Serial.print("spin time :");
-    Serial.println(spinTime);
-
-    motospd(-max_speedR, -max_speedR, -max_speedR);
-    delay(spinTime);
-    motospd(0, 0, 0);
-  }
-
-  if (anyCollision(25)) {  //check for collision before moving forward
-    motospd(0, -max_speed, max_speed);        // and move away if needed
-    delay(500);
-    motospd(0, 0, 0);
-  }
-  delay(50);
-}
-
-void solarRun(double solar_angle) {   // movement in the light
-
-  if (anyCollision(20)) {  //check for collision before moving
-    Serial.println("Moving away from wall");
-    getAwayFromWall();
-  }
-  boolean done = false;
-  updateSolar();
-  solarInfoDisplay();
-  for (int i = 0; i < 9; i++) {
-    if (solar[i] < 300) {
-      Serial.println("Moving towards the light");
-      solarRun2();               // solarRun2 moves toward the lighted area
-      done = true;               // if some sensors are too low
-      break;
-    }
-  }
-  if (!done) {
-    Serial.println("Attempting to center in the light");
-    solarRun3(solar_angle);  //already in the lighted are, so solarRun3
-  }                          //tries to center itself better in the light
-}
-
-// move towards the lighted area
-void solarRun2() {
-
-  updateSonar();
-  if (anyCollision(25)) {
-    getAwayFromWall();
-  }
-  motospd(0, max_speed, -max_speed); // its already facing the right direction
-  delay(1000);                     // so it just moves forward
-  motospd(0, 0, 0);
-
-  delay(2000);
-
-  getSun();    //recursive call to improve accuracy
-}
-
-void solarRun3(double solar_angle) { // centering in the lighted area
-  int min = 0;
-  for (int i = 1; i < 9; i++) { // find the smallest light sensor
-    if (solar[i] < solar[min]) {
-      min = i;
-    }
-  }
-  double back_theta = solar_angles[min] - 180; //move away from the smallest one
-  for (int i = 0; i < 3; i++) {
-    wheel_speed[i] = max_speed * newCos(wheel_angles[i] - back_theta);
-  }
-  motorWrite();
-  delay(1000);
-  motospd(0, 0, 0);
-
-  getSun();    // recursive call
-}
-
-boolean anyCollision(int distance_cm) { // check for a collision within a certain distance
-  updateSonar();                        // using the sonar in the front
-  sonarInfoDisplay();
-  for (int i = 0; i < 2; i++) {
-    if (cm[i] < distance_cm && cm[i] > 1) {
-      return true;
-    }
-  }
-  return false;
-}
-
-boolean anyCollision2() {   //check for a close range collision using the IR sensors
-  updateIRprox();
-  delay(50);
-  for (int i = 0; i < 4; i++) {
-    if (IR_proximity_state[i] < 524) {
-      return true;
-    }
-  }
-  return false;
-}
-
-void getAwayFromWall() {   //uses the IR sensors to move away from a detected obstacle
-  int sensorNum = colliSensorNumFeedback();
-  colliFeedbackMotion(sensorNum);
-}
-
-//give the number of the sensor which detected the obstacle
-int colliSensorNumFeedback() {
-  delay(50);
-  updateIRprox();
-  for (int i = 0; i < 4; i++) {
-    if (IR_proximity_state[i] < 300) {
-      Serial.print("backward_sensor:");
-      Serial.print(i);
-      Serial.println( );
-      return i;
-    }
-  }
-  Serial.println("No_backward_sensor");
-  return -1;
-}
-
-// uses the number of the obstacle sensor to move away from that direction
-void colliFeedbackMotion(int sensor_num) {
-  int num = sensor_num;
-  if (num == 0) {
-    motospd(-max_speed, 0, max_speed);
-    delay(2000); //backward 2s
-    motospd(0, 0, 0);
-    delay(10);
-  } else if (num == 1 || num == 2) {
-    motospd(0, -max_speed, max_speed);
-    delay(2000); //backward 2s
-    motospd(0, 0, 0);
-    delay(10);
-  } else if (num == 3) {
-    motospd(max_speed, -max_speed, 0);
-    delay(2000); //backward 2s
-    motospd(0, 0, 0);
-    delay(10);
-  } else {
-    return;
-  }
-}
-
-//writes motor speed values to motor
-// used to move in obscure directions
-void motorWrite() {
-  for (int i = 0; i < 3; i++) {
-    if (wheel_speed[i] < 0) {
-      digitalWrite(wheel_direction[i], LOW);
-    } else {
-      digitalWrite(wheel_direction[i], HIGH);
-    }
-    analogWrite(wheel_pwm[i], abs(wheel_speed[i]));
-  }
-}
-
-//direct way to assign values to motors
-//used for easy angles
-void motospd(int sp1, int sp2, int sp3) {
-  if (sp1 > 0) {
-    digitalWrite(wheel_direction[0], HIGH);
-  } else {
-    digitalWrite(wheel_direction[0], LOW);
-  }
-  if (sp2 > 0) {
-    digitalWrite(wheel_direction[1], HIGH);
-  } else {
-    digitalWrite(wheel_direction[1], LOW);
-  }
-  if (sp3 > 0) {
-    digitalWrite(wheel_direction[2], HIGH);
-  } else {
-    digitalWrite(wheel_direction[2], LOW);
-  }
-  analogWrite(wheel_pwm[0], abs(sp1));
-  analogWrite(wheel_pwm[1], abs(sp2));
-  analogWrite(wheel_pwm[2], abs(sp3));
-}
-
-//display updated wheel speed values
-void speedInfoDisplay() {
-  Serial.print("Speed Display: ");
-  for (int i = 0; i < 3; i++) {
-    Serial.print(wheel_speed[i]);
-    Serial.print("|");
-  }
-  Serial.println(" ");
-  Serial.println(" ");
-  delay(10);
-}
-
-//math tool
-//return the cosine value for an angle in degree
-double newCos(double phi) {
-  return cos(phi * M_PI / 180);
-}
-
-//return the sine value for an angle in degree
-double newSin(double phi) {
-  return sin(phi * M_PI / 180);
-}
-
-//return the tan value when you input y and x;
-//the outcome ranges from 0 to 360 degrees
-double newAtan(double x, double y) {
-  double theta = atan(y / x) * 180 / M_PI;
-  if (x < 0) {
-    return theta + 180;
-  } else if (y < 0) {
-    return theta + 360;
-  } else {
-    return theta;
-  }
-}
-
-//update solar panel values
-void updateSolar() {
-  for (int i = 0; i < 9; i++) {
-    solar[i] = analogRead(solar_pins[i]);
-    delay(50);
-  }
-}
-
-//display updated solar panel values
-void solarInfoDisplay() {
-  Serial.println("Solar Display: ");
-  for (int i = 0; i < 9; i++) {
-    Serial.print("Solar Pin ");
-    Serial.print(i);
-    Serial.print(" : ");
-    Serial.print(solar[i]);
-    Serial.print("     ");
-    delay(50);
-
-    Serial.println();
-  }
-}
-
-//update sonar sensor values in terms of distance
-void updateSonar() {
-  for (int i = 0; i < 6; i++) {
-    //cm[i]=sonar[i].ping_cm();
-    unsigned int uS = sonar[i].ping();
-    cm[i] = sonar[i].convert_cm(uS);
-    delay(50);
-
-    Serial.println(cm[i]);
-
-
-
-  }
-}
-
-//display updated sonar sensor values
-void sonarInfoDisplay() {
-  Serial.println("Distance Display: ");
-  for (int i = 0; i < 6; i++) {
-    Serial.print("Sonar Pin ");
-    Serial.print(i);
-    Serial.print(" : ");
-    Serial.println(cm[i]);
-  }
-  delay(50);
-}
-
-// update IR sensor states
-void updateIRprox() {
-  for (int i = 0; i < 4; i++) {
-    IR_proximity_state[i] = analogRead(IR_proximity_pins[i]);
-    delay(50);
-  }
-  delay(100);
-}
-
-// print updates IR sensor values
-void IRProxInfoDisplay() {
-  Serial.println("Proximity Display: ");
-  for (int i = 0; i < 4; i++) {
-    Serial.print("IR Pin ");
-    Serial.print(i);
-    Serial.print(" : ");
-    Serial.println(IR_proximity_state[i]);
-  }
-  delay(100);
-}
-
-
+//#include <Array.h>
+
+#include <SoftwareSerial.h>
+#include <SPI.h>
+#include <Pixy.h>
+#include <NewPing.h>
+#include <Math.h>
+#include <Time.h>
+#include <SD.h>
+
+#define X_CENTER 160L
+
+int wheel_pwm[] = {2,3,4}; //pins on motor drive to control speed
+int wheel_direction[] = {5,6,7}; //pins to control wheel direction
+double wheel_angles[] = {270, 30, 150}; //wheel angles are defined
+double wheel_speed[3]; //speed of each wheel is stored in this array
+double max_speed = 70; //maximum speed
+double max_speedR = 40; //rotate speed
+
+//create a new instance of Pixy
+Pixy pixy;
+
+double cm[2]; //sensed distance of sonar sensor : 2 represents array(2 sonar sensors)
+
+int trigger[6] = {30, 32, 46, 36, 38, 40}; //trigger pins assignment  (sends)
+int echo[6] = {31, 33, 47, 37, 39, 41}; //echo pins (receives)
+
+int colliDistance_cm = 25; //distances to define collision in cm/in
+int colliDistance_in = 12;
+
+//set up the sonar sensors for NewPing Library
+NewPing sonar[6] =
+{
+  NewPing(trigger[0], echo[0], 500), //500 represents type of sensor
+  NewPing(trigger[1], echo[1], 500),
+  NewPing(trigger[2], echo[2], 500),
+  NewPing(trigger[3], echo[3], 500),
+  NewPing(trigger[4], echo[4], 500),
+  NewPing(trigger[5], echo[5], 500),
+};
+
+int IR_proximity_pins[] = {A0, A1, A2, A3}; // pins for the proximity detection
+int IR_proximity_state[4];              // state of IR pins
+
+double solar[9]; //solar vectors are stored here
+double solar_angles[] = {290, 330, 10, 50, 90, 130, 170, 210, 250}; //solar angles defined
+int solar_pins[] = {A5, A7, A8, A9, A10, A12, A13, A14, A15}; // solar pins
+//analog pins assigned to solar panels
+
+boolean light1_state = false;
+boolean light2_state = false;
+boolean water_state = false;
+boolean CO2_state = false;
+
+const int motor_relay = 8;   //does not want relay to change.
+
+void setup() {     //does not return anything
+  Serial.begin(9600);  // begin serial to computer
+
+  for (int i = 0; i < 3; i++) {
+    pinMode(wheel_direction[i], OUTPUT); //wheel directions are OUTPUT
+  }
+  for (int i = 0; i < 4; i++) { //i++ for adding
+    pinMode(IR_proximity_pins[i], INPUT);
+  }
+  pinMode(motor_relay, OUTPUT);
+  digitalWrite(motor_relay, HIGH);
+
+  Serial3.begin(9600);    // begin serial to BASE ARDUINO
+  Serial1.begin(9600);    // begin serial to SD card
+
+  pixy.init();  // initialize pixy camera library functions
+  delay(100);  //delay of .1 seconds so that you do not compile errors
+  // find_red2();
+  // setTime(7,0,0,1,1,1);    // (hr,min,sec,day,month,yr)
+
+}
+/*
+  #include <SoftwareSerial.h>
+
+  SoftwareSerial mySerial(0, 1); // RX, TX
+
+  void setup()
+  {
+  // Open serial communications and wait for port to open:
+  Serial.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for Leonardo only
+  }
+
+
+  Serial.println("Goodnight moon!");
+
+  // set the data rate for the SoftwareSerial port
+  mySerial.begin(4800);
+  mySerial.println("Hello, world?");
+  }
+
+  void loop() // run over and over
+  {
+  if (mySerial.available())
+     Serial.write(mySerial.read());
+  if (Serial.available())
+     mySerial.write(Serial.read());
+  }
+*/
+void loop() {
+ // motospd(5,5,5);
+//Pixy_Align();
+//updateSonar();
+sonarInfoDisplay();
+//Do_Thingy();
+//Go_To_Object();
+//wheel_speeds(40,270);
+//motospd(0,0,40);
+  delay(1000);
+}
+void Do_Thingy(){
+  boolean aligned = false;
+  Serial.print(Am_I_Close());
+  if(!aligned){
+    Pixy_Align();
+    aligned = true;
+  }
+  else if (!Am_I_Close()){
+    Go_To_Object();
+  }
+  else{
+    return;
+  }
+}
+void Go_To_Object(){
+  if(Am_I_Close()){
+    motospd(0,0,0);
+  }
+  else{
+    motospd(0,40,-40);
+  }
+  delay(100);
+
+}
+boolean Am_I_Close(){
+   unsigned int uS = sonar[2].ping();
+    cm[1] = sonar[1].convert_cm(uS);
+    cm[2] = sonar[2].convert_cm(uS);
+    Serial.print(cm[1]);
+    Serial.print(cm[2]);
+    if (cm[1] <=30 && cm[1] != 0 || cm[2] <= 30 && cm[2] != 0){
+      return true;
+    }
+    else{
+      return false;
+    }
+
+}
+void Pixy_Align()
+{
+  Serial.println("-------------------------------------------------");
+  Serial.println("-------------------------------------------------");
+  get_coordinates();
+  int rotation_update_delay = 10;
+  int tolerance = 15;
+  int blocks = pixy.getBlocks(17);
+  if (pixy.blocks[0].x < (160 - tolerance) )
+  {
+    Serial.println("Rotating CoutnerClockwise...");
+    motospd(20, 20, 20);
+    delay(rotation_update_delay);
+    Pixy_Align();
+  }
+  else if (pixy.blocks[0].x > (160 + tolerance) )
+  {
+    Serial.println("Rotating Clockwise...");
+    motospd(-20, -20, -20);
+    delay(rotation_update_delay);
+    Pixy_Align();
+  }
+  else {
+//    Pixy_finetune();
+ Serial.println("pixy has been aligned");
+    motospd(0, 0, 0);
+    return;
+  }
+
+
+}
+
+void Pixy_finetune() {
+
+  Serial.println("------------||FINE TUNE IN PROGRESS||-----------");
+  Serial.println("-------------------------------------------------");
+  get_coordinates();
+  int rotation_update_delay = 10;
+  int tolerance = 5;
+  int spd = 10;
+  int blocks = pixy.getBlocks(17);
+  if (pixy.blocks[0].x < (160 - tolerance) )
+  {
+    Serial.println("Rotating CoutnerClockwise...");
+    motospd(spd, spd, spd);
+    delay(rotation_update_delay);
+    Pixy_finetune();
+  }
+  else if (pixy.blocks[0].x > (160 + tolerance) )
+  {
+    Serial.println("Rotating Clockwise...");
+    motospd(-1 * spd, -1 * spd, -1 * spd);
+    delay(rotation_update_delay);
+    Pixy_finetune();
+  }
+  else {
+    Serial.println("pixy has been aligned");
+    motospd(0, 0, 0);
+    return;
+  }
+
+}
+
+void get_coordinates()
+{
+
+  int blocks = pixy.getBlocks(17);
+  for (int k = 0; k < blocks; k++) {
+    if (pixy.blocks[k].signature == 1) {
+      Serial.print("The X coordinate is:");
+      Serial.print(pixy.blocks[k].x);
+      Serial.println();
+      Serial.print("The Y coordinate is:");
+      Serial.print(pixy.blocks[k].y);
+      Serial.println();
+
+    }
+  }
+
+}
+void wheel_speeds(float tot_spd, float ang){   //Input speed and direction you want to go from 0 to 360
+  float results[4]; //establish the results arrray for relative angle.
+  Relative_ang(results,ang); //returns relative angle from -30 to 30 and wheels. First two wheels are main wheels, last wheel compensates for directional movement
+
+  float Motor_2 = abs(((((-1/sqrt(3))*tan((results[0]/180)*M_PI))-1)*(tot_spd/(1/cos((results[0]/180)*M_PI))))); //maths
+  float Motor_1 = abs(((((1/sqrt(3))*tan((results[0]/180)*M_PI))-1)*(tot_spd/(1/cos((results[0]/180)*M_PI)))));  //more maths
+
+  float Motor_Comp = .0018*pow(results[0],3) - 0.1019*pow(results[0],2) + 2.446*results[0] + 0.8571; //equaiton for compensator found by experiment and excel curve fit
+
+
+
+//----------------------------------------------------------------------------------------
+//READ ME!!!!!!!
+//
+//What is done below may look confusing, and it is to an extent. This method was done in order to not have a bunch of if/else statements
+//
+// The general idea: Relative_ang() gives values to the array called results. results is an array where the first position is the relative angle and the following
+// three are motors, with the first two being main working motors and the last being a compensator for the direction
+//
+// What we need to do is assign each of these wheels the proper wheel speed. Using the knowledge that the first two motors listed in the results array are the
+// main motors, and that the motors are called (+/-)1,2,3.....we can give the proper value to the proper motor. Since motospd(...,...,...) takes in three values that
+// are always in order from wheel 1 to 3,  we must assign the wheels from results an array value and then assign the proper speed at that specific array value and
+  // asign it to an array that reorganizes the wheels so motospd can distribute the proper speed to the proper wheel....... this is a shitty explanation pls forgive me
+  //
+  //
+int a = abs(results[1])-1; //since the wheels are labled from 1 to 3, we subtract one to give it a proper position in the motors array below
+int b = abs(results[2])-1; // e.g. results[2] = wheel 3.......3-1 = 2.... 'b' will be in the position of motors[2]
+int c = abs(results[3])-1;
+
+ float motors[3]; // establish the final wheel speed array to set wheels
+ motors[a]= (results[1]/(abs(results[1]))) * Motor_1;  //(results[1]/(abs(results[1]))) is to determine the directon of the wheel rotation (positive or negative)
+ motors[b]= (results[2]/(abs(results[2]))) * Motor_2;
+ motors[c]= (results[3]/(abs(results[3]))) * Motor_Comp; // so since 'c' corresponds to the value of the wheel in results[3], the compensator value is assigned.
+ // this does not mean motor[c] will always be the thrid value in motors. if the compensator wheel is determined to be wheel 2, then 2-1 = 1, and motors[1] will be assinged
+ // the compenator value, which is the second value in the array for this case.
+
+Serial.println(results[0]);
+Serial.println(results[1]);
+Serial.println(results[2]);
+Serial.println(results[3]);
+Serial.println("------------------------------------");
+Serial.println("--------------Motors---------------");
+Serial.println("------------------------------------");
+Serial.println(motors[0]);
+Serial.println(motors[1]);
+Serial.println(motors[2]);
+Serial.println("------------------------------------");
+Serial.println("--------------Results---------------");
+Serial.println("------------------------------------");
+
+ motospd(motors[0],motors[1],motors[2]); //set all the wheel seeds
+}
+
+
+void Relative_ang(float *ptr, float ang){
+//float a,b,c,d;
+// float new_config[4]={0,0,0,0};
+// float *ptr[4];
+ float new_ang;
+
+
+    if(ang >= 0 && ang<= 30){
+      new_ang = ang;
+     //new_config = {new_ang, 3,2,1};
+      ptr[0] = new_ang;
+      ptr[1] = -3;
+      ptr[2] = 2;
+      ptr[3] = -1;
+
+    }
+     else if(ang > 330 && ang<= 360){
+      new_ang = ang;
+     //new_config = {new_ang, 3,2,1};
+      ptr[0] = new_ang - 360;
+      ptr[1] = -3;
+      ptr[2] = 2;
+      ptr[3] = 1;
+
+    }
+    else if (ang > 30 && ang <= 90){
+     new_ang = ang - 60;
+    //new_config = {new_ang, -2,-1,3};
+     ptr[0] = new_ang;
+      ptr[1] = 2;
+      ptr[2] = -1;
+      ptr[3] = -1*(new_ang/abs(new_ang))*3;
+    }
+    else if (ang > 90 && ang <= 150){
+      new_ang = ang - 120;
+     //new_config = {new_ang, 3,1,2};
+      ptr[0] = new_ang;
+      ptr[1] = 3;
+      ptr[2] = -1;
+      ptr[3] = -1*(new_ang/abs(new_ang))*2;
+    }
+    else if(ang > 150 && ang<= 210){
+      new_ang = ang - 180;
+     // new_config = {new_ang, -3,-2,1};
+      ptr[0] = new_ang;
+      ptr[1] = 3;
+      ptr[2] = -2;
+      ptr[3] = -1*(new_ang/abs(new_ang))*1;
+    }
+    else if(ang > 210 && ang<= 270){
+      new_ang = ang - 240;
+     // new_config = {new_ang, 2,1,3};
+      ptr[0] = new_ang;
+      ptr[1] = -2;
+      ptr[2] = 1;
+      ptr[3] = -1*(new_ang/abs(new_ang))*3;
+    }
+    else if(ang > 270 && ang<= 330){
+      new_ang = ang - 300;
+     // new_config = {new_ang, -3,-1,2};
+      ptr[0] = new_ang;
+      ptr[1] = -3;
+      ptr[2] = 1;
+      ptr[3] = -1*(new_ang/abs(new_ang))*2;
+     }
+
+
+ return 0;
+
+
+}
+
+//float Relative_ang(float ang){
+// //float a,b,c,d;
+// float new_config[4]={0,0,0,0};
+// float *ptr[4];
+//  float new_ang;
+//
+//
+//     if((ang > 330 && ang<= 360) || (ang >= 0 && ang<= 30)){
+//       new_ang = ang;
+//      new_config = {new_ang, 3,2,1};
+//        a= new_ang;
+//
+//
+//     }
+//     else if (ang > 30 && ang <= 90){
+//      new_ang = ang + 300;
+//     new_config = {new_ang, -2,-1,3};
+//     }
+//     else if (ang > 90 && ang <= 150){
+//       new_ang = ang + 240;
+//      new_config = {new_ang, 3,1,2};
+//     }
+//     else if(ang > 150 && ang<= 210){
+//       new_ang = ang + 180;
+//      new_config = {new_ang, -3,-2,1};
+//     }
+//     else if(ang > 210 && ang<= 270){
+//       new_ang = ang + 120;
+//      new_config = {new_ang, 2,1,3};
+//     }
+//     else if(ang > 270 && ang<= 330){
+//       new_ang = ang + 60;
+//      new_config = {new_ang, -3,-1,2};
+//      }
+//
+//  for(int i = 0; i < 4; i++){
+//    ptr[i] = &new_config[i]; // assign the address of integer.
+//  }
+//  return **ptr;
+//
+//
+//}
+
+
+
+void Lets_Move(double ang, int tot_spd, int comp){
+float Motor_2;
+float Motor_1;
+
+  Motor_2 = ((((-1/sqrt(3))*tan((ang/180)*M_PI))-1)*(tot_spd/(1/cos((ang/180)*M_PI))));
+  Motor_1 = -1*((((1/sqrt(3))*tan((ang/180)*M_PI))-1)*(tot_spd/(1/cos((ang/180)*M_PI))));
+  Serial.println("-----------------------");
+  Serial.println("-----------------------");
+  Serial.print("Motor 1 Speed:");
+  Serial.println(Motor_1);
+  Serial.print("Motor 2 Speed:");
+  Serial.println(Motor_2);
+
+ motospd(comp,Motor_1,Motor_2); //At Min Speed, set compensator to 18.
+ delay(1000);
+}
+
+// Just a mini test cycle to try and visualize each station without having to wait
+// the actual time it takes for lighting or getting CO2 or whatever its doing
+void test_cycle() {
+  delay(2000);   //waits 2 seconds
+  getWater(1);   //gets water for a minute
+  delay(1000);   //waits 10 seconds
+  Serial.println("preparing to measure moisture");
+  Serial1.write('M');
+  delay(1000);   //waits 1 second
+  delay(1000);                  //turn on lights
+  Serial3.println("7H2");
+  delay(3000); //waits 3 seconds
+  light1_state = true;
+  getSun();
+  Serial3.println("7H1");
+  delay(120000);
+  Serial3.println("7H2");
+  delay(3000);
+  light1_state = true;
+  getSun();
+  Serial3.println("7H1");
+  delay(120000);
+  getCO2(1);
+
+}
+
+// cycle for the entire day
+void day_cycle() {
+  getWater(1);
+  delay(500);
+  Serial.println("preparing to measure moisture");
+  Serial1.write('M');
+
+  // measure after getting water, then again 2 full cycles later
+  for (int i = 0; i < 7; i++) {
+    delay(100);
+    mini_cycle();
+    delay(100);
+    if (i == 2) {
+      Serial.println("preparing to measure moisture");
+      Serial1.write('M');
+    }
+    delay(100);
+  }
+}
+
+//run the full 5 light cycle then get CO2
+void mini_cycle() {
+  light_cycle();
+  delay(2000);
+  getCO2(30);
+  delay(500);
+}
+
+// single light cycle just gets light once then sits
+void light_cycle() { //cycling light
+  for (int i = 0; i < 5; i++) {
+    light_loop();
+    delay(1800000);       //no sun for 30 min
+    motospd(30, 30, 30);  // after getting in a lit position
+    delay(1100);          // rotate a little after each cycle
+    motospd(0, 0, 0);     // to try and be more distributed
+  }
+}
+
+void light_loop() {
+  delay(50);                  //turn on lights
+  Serial3.println("7H3");
+  delay(3000);
+  light1_state = true;   //light is on now because it is true
+  getSun();
+  Serial3.println("7H5");
+  delay(300000);          //get sun for 5 min
+}
+
+void getWater(int water_time) { //time to get water in minutes
+  find_red1();   //indicator for detecting
+  delay(1000);
+  String msg = "5H";
+  msg = msg + water_time;
+  Serial3.println(msg);
+  water_state = true;
+  delay(water_time * 30000);
+  Serial3.println("5L");
+  delay(2000);
+  water_state = false;
+  delay(2000);
+  backAwayFromRed();
+}
+
+void getCO2(int CO2_time) {   // time to sit in CO2 cage in minutes
+  find_green1();
+  delay(1000);
+  String msg = "4H";
+  msg = msg + CO2_time;
+  Serial3.println(msg);  //writes to arduino
+  CO2_state = true;
+  delay(60000);
+  delay(CO2_time * 60000);
+  delay(2000);
+  delay(60000);
+  CO2_state = false;
+  backAwayFromGreen();
+}
+
+void find_red1() {   // parent function to call recursive functions and talk to BASE
+  // turn on the lights temporarily for tracking
+  Serial.println("Searching for red");
+  delay(1000);
+  Serial3.println("6H3");
+  delay(50);
+  light2_state = true;
+
+  digitalWrite(motor_relay, LOW); //turn motors on
+  delay(250);
+
+  find_red2();       // locate, center, and approach target
+  horizontalCenter();// horiztonally center along the wall
+  adjustRed();       // adjust distance to wall to line up with water faucet
+
+  digitalWrite(motor_relay, HIGH); //turn motors off
+  delay(100);
+  Serial3.println("6L");
+  delay(1000);
+}
+
+void find_red2() {
+  motospd(20, 20, 20);    // spin until red is detected
+  locate_red();
+  Serial.println("red detected");
+  /*
+    centerObject2(1);    // center with target
+
+    delay(50);
+
+    static int i = 0;  //only can be changed outside the global scope; can't be changed in a for loop
+    int j;
+    uint16_t blocks;   //allocated integer of 16 bits
+    char buf[32];    //character
+    int32_t xError;
+    blocks = pixy.getBlocks(17);
+
+    int height = pixy.blocks[0].height;;
+    Serial.print("height:  ");
+    Serial.println(height); //prints the height of the variable height
+
+    double dist_away = 1866.0 / height;    // use height to get distance
+    //distance away in inches
+
+    if (dist_away > 100 || dist_away < 0) {      // throw out error when pixy picks up
+     motospd(max_speedR,max_speedR,max_speedR); // small background objects
+     delay(500);
+     motospd(0,0,0);  //not moving
+     find_red2();
+     return;
+    }
+    Serial.print("distance away:  ");
+    Serial.println(dist_away);
+
+    pixyRun2(dist_away, 1);    //approach target
+    delay(10);*/
+}
+
+void locate_red() {    //spin and refresh camera until red is detected
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+  int32_t xError;
+  blocks = pixy.getBlocks(17);
+
+  for (int k = 0; k < blocks; k++) {
+    Serial.println("for loop started");
+    if (pixy.blocks[k].signature == 1) {
+      Serial.println("found block");
+      delay(500);
+      motospd(0, 0, 0);
+      delay(5000);
+      return;
+    }
+  }
+  delay(50);
+  locate_red();
+}
+
+void backAwayFromRed() {     // back away from red wall before performing next task
+  digitalWrite(motor_relay, LOW); //low means on in this case
+  delay(250);
+
+  motospd(0, -max_speed, max_speed);
+  delay(2000);
+  motospd(0, 0, 0);
+
+  digitalWrite(motor_relay, HIGH);
+}
+
+void pixyAlignRed() {     //align straight with the red target
+  Serial.println("Aligning with object");
+  updateSonar();
+  delay(100);
+  updateSonar();
+  sonarInfoDisplay();
+
+  if (abs(cm[0] - cm[1]) < 2) { //spin left or right based on sonar values
+    Serial.println("Aligned");
+    return;
+  } else if (cm[0] > cm[1]) {
+    motospd(40, 10, 10);
+    delay(250);
+    motospd(0, 0, 0);
+    delay(100);
+    pixyAlignRed();
+  } else {
+    motospd(-40, -10, -10);
+    delay(250);
+    motospd(0, 0, 0);
+    delay(100);
+    pixyAlignRed();
+  }
+}
+
+void adjustRed() {   // adjust distance from red to line up with water faucet
+  Serial.println("adjusting");
+  updateSonar();
+  delay(100);
+  updateSonar();
+
+  double adjust = 24 - ((cm[0] + cm[1]) / 2.0);
+  Serial.print("adjust distance: ");
+  Serial.println(adjust);
+
+  if (adjust > 0) {  // move forward or backward based on sonar
+    motospd(0, -max_speed, max_speed);
+    double adjustTime = adjust / 2.54 / 11 * 1000;
+    delay(adjustTime);
+    motospd(0, 0, 0);
+  } else {
+    adjust = -adjust;
+    motospd(0, max_speed, -max_speed);
+    double adjustTime = adjust / 2.54 / 11 * 1000;
+    delay(adjustTime);
+    motospd(0, 0, 0);
+  }
+}
+
+void horizontalCenter() {   // center left or right along the wall to line up with water
+  Serial.println("horizontally center/align to object");
+
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+  int32_t xError;
+  blocks = pixy.getBlocks(17);
+
+  int center = pixy.blocks[0].x;    // use pixy x-axis center function to line it up
+  Serial.print("center: ");
+  Serial.println(center);
+
+  if (abs(center - 150) < 5) {
+    Serial.println("hor. centered with the object - ready for dist. adjust");
+    return;
+  } else if (center < 150) {
+    Serial.println("hor. center - moving left");
+    for (int i = 0; i < 3; i++) {
+      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 90);
+    }
+    motorWrite();
+    delay(200);
+    motospd(0, 0, 0);
+    delay(1000);
+    horizontalCenter();
+  } else {
+    Serial.println("hor. center - moving right");
+    for (int i = 0; i < 3; i++) {
+      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 270);
+    }
+    motorWrite();
+    delay(200);
+    motospd(0, 0, 0);
+    delay(1000);
+    horizontalCenter();
+  }
+
+}
+
+void find_green1() {   // parent function to call recursive functions and talk to BASE
+  delay(1500);           // turn on lights temporarily for color tracking
+  Serial3.println("6H5");
+  delay(1000);
+  light2_state = true;
+
+  digitalWrite(motor_relay, LOW);
+  delay(250);
+
+  find_green2();       // locate, center, and approach green
+  horizontalCenterGreen();   // hor line up to object before adjust
+  adjustGreen();       // adjust distance to green to line up with CO2 cage
+
+  digitalWrite(motor_relay, HIGH);
+  delay(100);
+  Serial3.println("6L");
+  delay(1000);
+}
+
+//recursive function to narrow in on the green
+void find_green2() {
+  motospd(-max_speedR, -max_speedR, -max_speedR);    //spin until green is detected
+  delay(300);
+  locate_green();
+  Serial.println("green detected");
+
+
+  centerObject2(3);    // center with green
+
+  delay(50);
+
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+  int32_t xError;
+  blocks = pixy.getBlocks(17);
+
+  int height;
+  //height = pixy.blocks[0].height;
+
+  if (blocks == 1) {
+    height = pixy.blocks[0].height;
+  } else {
+    height = pixy.blocks[1].height;
+  }
+
+  Serial.print("height:  ");
+  Serial.println(height);
+
+  double dist_away = 1800.0 / height;    // use height to find distance away
+  //distance away in inches
+
+  if (dist_away > 100 || dist_away < 0) {  //get rid of background noise detection errors
+    find_green2();
+    return;
+  }
+  Serial.print("distance away:  ");
+  Serial.println(dist_away);
+
+  pixyRun2(dist_away, 3);    // approach target
+  delay(10);
+}
+
+void locate_green() {  //spin and refresh camera until green is detected
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+  int32_t xError;
+  blocks = pixy.getBlocks(17);
+
+  for (int k = 0; k < blocks; k++) {
+    if (pixy.blocks[k].signature == 3) {
+      delay(300);
+      motospd(0, 0, 0);
+      return;
+    }
+  }
+  delay(50);
+  locate_green();
+}
+
+void centerObject2(int color_num) {    // recursive center with target
+
+  Serial.println("centering");
+  delay(50);
+
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+
+  blocks = pixy.getBlocks(17);
+
+  int block_num;
+  int error;  //error if you do not input 1 or 0
+  if (blocks) {
+
+    if (color_num == 1) {
+      block_num = 0;
+    } else {
+      if (blocks == 1) {
+        block_num = 0;
+      } else {
+        block_num = 1;
+      }
+    }
+
+    int height = pixy.blocks[block_num].height;
+    if (height < 10) {
+      if (color_num == 1) {
+        find_red2();
+      } else {
+        find_green2();
+      }
+      return;
+    }
+
+    uint32_t center = pixy.blocks[block_num].x;    // use x-axis center function to align
+    error = X_CENTER - center;                     // robot with target
+    Serial.print("center: ");
+    Serial.print(center);
+    Serial.print("   error: ");
+    Serial.println(error);
+  }
+
+  if (abs(error) < 10) {
+    Serial.println("centered");    // <10 is good enough alignment with margin of error
+    return;
+  }
+
+  int spin_time = abs(error) * 4.5;    // spin time based on size of center error
+  Serial.print("Spin Time:  ");
+  Serial.println(spin_time);
+
+  if (error >= 0) {
+    motospd(max_speedR, max_speedR, max_speedR);
+  } else {
+    motospd(-max_speedR, -max_speedR, -max_speedR);
+  }
+  delay(spin_time);
+  motospd(0, 0, 0);
+
+  centerObject2(color_num);    //recursive call to improve accuracy
+}
+
+void pixyRun2(double dist_away, int color_num) {   //approach target
+  if (anyCollision(5)) {  // if theres a collision, move away
+    getAwayFromWall();
+    if (color_num == 1) {
+      find_red2();
+      return;
+    } else if (color_num == 3) {
+      find_green2();
+      return;
+    }
+  }
+  if (color_num == 1) {    // distance for red is 15 in
+    if (dist_away < 15) {
+      pixyAlignRed();
+      return;
+    }
+  } else if (color_num == 3) { //distance for green is 20 in
+    if (dist_away < 20) {
+      pixyAlignGreen();
+      return;
+    }
+  }
+
+  double approach_time = dist_away / 11 * 1000.0;  // approach time based on distance
+  motospd(0, max_speed, -max_speed);
+  if (color_num == 1) {
+    delay(approach_time / 3.0);    // move part of the way
+  } else if (color_num == 3) {
+    delay(approach_time / 3.0);
+  }
+  motospd(0, 0, 0);
+  delay(10);
+
+  if (color_num == 3) {  //recursive call to continue process
+    delay(50);
+    find_green2();
+  } else if (color_num == 1) {
+    delay(50);
+    find_red2();
+  }
+}
+
+void backAwayFromGreen() {
+  digitalWrite(motor_relay, LOW);
+  delay(250);
+
+  motospd(0, -max_speed, max_speed);
+  delay(4500);
+  motospd(0, 0, 0);
+
+  digitalWrite(motor_relay, HIGH);
+}
+
+void pixyAlignGreen() { //align robot with green to line up with CO2 cage
+  Serial.println("Aligning with object");
+  updateSonar();
+  sonarInfoDisplay();
+
+  if (abs(cm[0] - cm[1]) < 3) { // move left or right based on sonar
+    return;
+  } else if (cm[0] > cm[1]) {
+    motospd(40, 10, 10);
+    delay(300);
+    motospd(0, 0, 0);
+    delay(100);
+    pixyAlignGreen();
+  } else {
+    motospd(-40, -10, -10);
+    delay(300);
+    motospd(0, 0, 0);
+    delay(100);
+    pixyAlignGreen();
+  }
+}
+
+void horizontalCenterGreen() {   // center left/right along the wall to line up with water
+  Serial.println("horizontally center/align to object");
+
+  static int i = 0;
+  int j;
+  uint16_t blocks;
+  char buf[32];
+  int32_t xError;
+  blocks = pixy.getBlocks(17);
+
+  int center = pixy.blocks[0].x;    // use pixy x-axis center function to line it up
+  Serial.print("center: ");
+  Serial.println(center);
+
+  if (abs(center - 150) < 5) {
+    Serial.println("hor. centered with the object - ready for dist. adjust");
+    return;
+  } else if (center < 150) {
+    Serial.println("hor. center - moving left");
+    for (int i = 0; i < 3; i++) {
+      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 90);
+    }
+    motorWrite();
+    delay(200);
+    motospd(0, 0, 0);
+    delay(1000);
+    horizontalCenterGreen();
+  } else {
+    Serial.println("hor. center - moving right");
+    for (int i = 0; i < 3; i++) {
+      wheel_speed[i] = max_speed * newCos(wheel_angles[i] - 270);
+    }
+    motorWrite();
+    delay(200);
+    motospd(0, 0, 0);
+    delay(1000);
+    horizontalCenterGreen();
+  }
+}
+
+
+void adjustGreen() {   // adjust distance to green to line up with CO2 cage
+  updateSonar();
+
+  double adjust = 48 - ((cm[0] + cm[1]) / 2.0);
+  if (adjust > 0) {  // move forward or backward based on sonar
+    motospd(0, -max_speed, max_speed);
+    double adjustTime = adjust / 2.54 / 11 * 1000;
+    delay(adjustTime);
+    motospd(0, 0, 0);
+  } else {
+    adjust = -adjust;
+    motospd(0, max_speed, -max_speed);
+    double adjustTime = adjust / 2.54 / 11 * 1000;
+    delay(adjustTime);
+    motospd(0, 0, 0);
+  }
+}
+
+void getSun() {  // find light
+  digitalWrite(motor_relay, LOW); //turn on the motor
+
+  updateSolar();
+  delay(100);
+  solarInfoDisplay();
+
+  if (evenSun()) {  // if its evenly distributed in the sun then its done
+    digitalWrite(motor_relay, HIGH);
+    return;
+  }
+
+  double solar_angle = getSolarAngle();  //get angle of light
+  Serial.print("Solar Angle:  ");
+  Serial.println(solar_angle);
+
+  delay(2000);
+
+  rotateToLight2(solar_angle);  //rotate "front" of robot towards that solar angle
+
+  solarRun(solar_angle);    // approach light
+
+}
+
+//calculate the solar angle in degree by
+//using all 3 solar vectors the outcome
+//ranges from 0degree to 360degree
+double getSolarAngle() {
+  double x = 0;
+  double y = 0;
+  for (int i = 0; i < 9; i++) {
+    x += solar[i] * newCos(solar_angles[i]);
+    y += solar[i] * newSin(solar_angles[i]);
+  }
+  return newAtan(x, y);
+}
+
+// see if all the solar sensors are within range of each other
+// to see if its evenly distributed
+boolean evenSun() {
+  for (int i = 0; i < 9; i++) {
+    for (int j = i + 1; j < 9; j++) {
+      if (abs(solar[i] - solar[j]) > 250) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+void rotateToLight(double solar_angle) {   //rotate towards the given angle
+
+  double rotationAngle = solar_angle - 90;
+  if (rotationAngle < 0) {
+    rotationAngle += 360;
+  }
+
+  double spinTime = rotationAngle * 13000 / 360;
+  Serial.print("spin time :");
+  Serial.println(spinTime);
+
+  motospd(max_speedR, max_speedR, max_speedR);
+  delay(spinTime);
+  motospd(0, 0, 0);
+
+  if (anyCollision(25)) {
+    motospd(0, -max_speed, max_speed);
+    delay(500);
+    motospd(0, 0, 0);
+  }
+  delay(50);
+}
+
+void rotateToLight2(double solar_angle) {   // rotate to the solar angle
+  if (solar_angle >= 90 && solar_angle <= 270) {  // if its in this half of the circle,
+    double rotationAngle = solar_angle - 90;      // then rotate CCW
+
+    double spinTime = rotationAngle * 13000 / 360;
+    Serial.print("spin time :");
+    Serial.println(spinTime);
+
+    motospd(max_speedR, max_speedR, max_speedR);
+    delay(spinTime);
+    motospd(0, 0, 0);
+  } else {               // if its in the other half of the circle, then rotate CW
+    double rotationAngle;
+    if (solar_angle < 90) {
+      rotationAngle = 90 - solar_angle;
+    } else {
+      rotationAngle = 90 + (360 - solar_angle);
+    }
+    double spinTime = rotationAngle * 13000 / 360;
+    Serial.print("spin time :");
+    Serial.println(spinTime);
+
+    motospd(-max_speedR, -max_speedR, -max_speedR);
+    delay(spinTime);
+    motospd(0, 0, 0);
+  }
+
+  if (anyCollision(25)) {  //check for collision before moving forward
+    motospd(0, -max_speed, max_speed);        // and move away if needed
+    delay(500);
+    motospd(0, 0, 0);
+  }
+  delay(50);
+}
+
+void solarRun(double solar_angle) {   // movement in the light
+
+  if (anyCollision(20)) {  //check for collision before moving
+    Serial.println("Moving away from wall");
+    getAwayFromWall();
+  }
+  boolean done = false;
+  updateSolar();
+  solarInfoDisplay();
+  for (int i = 0; i < 9; i++) {
+    if (solar[i] < 300) {
+      Serial.println("Moving towards the light");
+      solarRun2();               // solarRun2 moves toward the lighted area
+      done = true;               // if some sensors are too low
+      break;
+    }
+  }
+  if (!done) {
+    Serial.println("Attempting to center in the light");
+    solarRun3(solar_angle);  //already in the lighted are, so solarRun3
+  }                          //tries to center itself better in the light
+}
+
+// move towards the lighted area
+void solarRun2() {
+
+  updateSonar();
+  if (anyCollision(25)) {
+    getAwayFromWall();
+  }
+  motospd(0, max_speed, -max_speed); // its already facing the right direction
+  delay(1000);                     // so it just moves forward
+  motospd(0, 0, 0);
+
+  delay(2000);
+
+  getSun();    //recursive call to improve accuracy
+}
+
+void solarRun3(double solar_angle) { // centering in the lighted area
+  int min = 0;
+  for (int i = 1; i < 9; i++) { // find the smallest light sensor
+    if (solar[i] < solar[min]) {
+      min = i;
+    }
+  }
+  double back_theta = solar_angles[min] - 180; //move away from the smallest one
+  for (int i = 0; i < 3; i++) {
+    wheel_speed[i] = max_speed * newCos(wheel_angles[i] - back_theta);
+  }
+  motorWrite();
+  delay(1000);
+  motospd(0, 0, 0);
+
+  getSun();    // recursive call
+}
+
+boolean anyCollision(int distance_cm) { // check for a collision within a certain distance
+  updateSonar();                        // using the sonar in the front
+  sonarInfoDisplay();
+  for (int i = 0; i < 2; i++) {
+    if (cm[i] < distance_cm && cm[i] > 1) {
+      return true;
+    }
+  }
+  return false;
+}
+
+boolean anyCollision2() {   //check for a close range collision using the IR sensors
+  updateIRprox();
+  delay(50);
+  for (int i = 0; i < 4; i++) {
+    if (IR_proximity_state[i] < 524) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void getAwayFromWall() {   //uses the IR sensors to move away from a detected obstacle
+  int sensorNum = colliSensorNumFeedback();
+  colliFeedbackMotion(sensorNum);
+}
+
+//give the number of the sensor which detected the obstacle
+int colliSensorNumFeedback() {
+  delay(50);
+  updateIRprox();
+  for (int i = 0; i < 4; i++) {
+    if (IR_proximity_state[i] < 300) {
+      Serial.print("backward_sensor:");
+      Serial.print(i);
+      Serial.println( );
+      return i;
+    }
+  }
+  Serial.println("No_backward_sensor");
+  return -1;
+}
+
+// uses the number of the obstacle sensor to move away from that direction
+void colliFeedbackMotion(int sensor_num) {
+  int num = sensor_num;
+  if (num == 0) {
+    motospd(-max_speed, 0, max_speed);
+    delay(2000); //backward 2s
+    motospd(0, 0, 0);
+    delay(10);
+  } else if (num == 1 || num == 2) {
+    motospd(0, -max_speed, max_speed);
+    delay(2000); //backward 2s
+    motospd(0, 0, 0);
+    delay(10);
+  } else if (num == 3) {
+    motospd(max_speed, -max_speed, 0);
+    delay(2000); //backward 2s
+    motospd(0, 0, 0);
+    delay(10);
+  } else {
+    return;
+  }
+}
+
+//writes motor speed values to motor
+// used to move in obscure directions
+void motorWrite() {
+  for (int i = 0; i < 3; i++) {
+    if (wheel_speed[i] < 0) {
+      digitalWrite(wheel_direction[i], LOW);
+    } else {
+      digitalWrite(wheel_direction[i], HIGH);
+    }
+    analogWrite(wheel_pwm[i], abs(wheel_speed[i]));
+  }
+}
+
+//direct way to assign values to motors
+//used for easy angles
+void motospd(int sp1, int sp2, int sp3) {
+  if (sp1 > 0) {
+    digitalWrite(wheel_direction[0], HIGH);
+  } else {
+    digitalWrite(wheel_direction[0], LOW);
+  }
+  if (sp2 > 0) {
+    digitalWrite(wheel_direction[1], HIGH);
+  } else {
+    digitalWrite(wheel_direction[1], LOW);
+  }
+  if (sp3 > 0) {
+    digitalWrite(wheel_direction[2], HIGH);
+  } else {
+    digitalWrite(wheel_direction[2], LOW);
+  }
+  analogWrite(wheel_pwm[0], abs(sp1));
+  analogWrite(wheel_pwm[1], abs(sp2));
+  analogWrite(wheel_pwm[2], abs(sp3));
+}
+
+//display updated wheel speed values
+void speedInfoDisplay() {
+  Serial.print("Speed Display: ");
+  for (int i = 0; i < 3; i++) {
+    Serial.print(wheel_speed[i]);
+    Serial.print("|");
+  }
+  Serial.println(" ");
+  Serial.println(" ");
+  delay(10);
+}
+
+//math tool
+//return the cosine value for an angle in degree
+double newCos(double phi) {
+  return cos(phi * M_PI / 180);
+}
+
+//return the sine value for an angle in degree
+double newSin(double phi) {
+  return sin(phi * M_PI / 180);
+}
+
+//return the tan value when you input y and x;
+//the outcome ranges from 0 to 360 degrees
+double newAtan(double x, double y) {
+  double theta = atan(y / x) * 180 / M_PI;
+  if (x < 0) {
+    return theta + 180;
+  } else if (y < 0) {
+    return theta + 360;
+  } else {
+    return theta;
+  }
+}
+
+//update solar panel values
+void updateSolar() {
+  for (int i = 0; i < 9; i++) {
+    solar[i] = analogRead(solar_pins[i]);
+    delay(50);
+  }
+}
+
+//display updated solar panel values
+void solarInfoDisplay() {
+  Serial.println("Solar Display: ");
+  for (int i = 0; i < 9; i++) {
+    Serial.print("Solar Pin ");
+    Serial.print(i);
+    Serial.print(" : ");
+    Serial.print(solar[i]);
+    Serial.print("     ");
+    delay(50);
+
+    Serial.println();
+  }
+}
+
+//update sonar sensor values in terms of distance
+void updateSonar() {
+  for (int i = 0; i < 6; i++) {
+    //cm[i]=sonar[i].ping_cm();
+    unsigned int uS = sonar[i].ping();
+    cm[i] = sonar[i].convert_cm(uS);
+    delay(50);
+
+    Serial.println(cm[i]);
+
+
+
+  }
+}
+
+//display updated sonar sensor values
+void sonarInfoDisplay() {
+  Serial.println("Distance Display: ");
+  for (int i = 0; i < 6; i++) {
+    Serial.print("Sonar Pin ");
+    Serial.print(i);
+    Serial.print(" : ");
+    Serial.println(cm[i]);
+  }
+  delay(50);
+}
+
+// update IR sensor states
+void updateIRprox() {
+  for (int i = 0; i < 4; i++) {
+    IR_proximity_state[i] = analogRead(IR_proximity_pins[i]);
+    delay(50);
+  }
+  delay(100);
+}
+
+// print updates IR sensor values
+void IRProxInfoDisplay() {
+  Serial.println("Proximity Display: ");
+  for (int i = 0; i < 4; i++) {
+    Serial.print("IR Pin ");
+    Serial.print(i);
+    Serial.print(" : ");
+    Serial.println(IR_proximity_state[i]);
+  }
+  delay(100);
+}
+
+


alarm sounding and cancellation

alarm sounding and cancellation

--- SmartAlarm.ino
+++ SmartAlarm.ino
@@ -48,6 +48,8 @@
 #define KEY_DOWN 1
 #define KEY_UP 2
 #define KEY_OK 8
+// random defines
+#define NALARMS 10

 DateTime now = DateTime(0);

@@ -167,10 +169,16 @@

 }

+typedef struct {
+  DateTime set;
+  bool active;
+  bool sounding;
+} alarm;
+
 /**
  * raises an alarm
  */
-class Alarm {
+class AlarmSounder {
   unsigned long PrevMillis;
   unsigned long DurationOn; // in millis
   unsigned long DurationOff; // in millis
@@ -178,14 +186,14 @@
   uint8_t pin;

 public:
-  Alarm(unsigned long dOn, unsigned long dOff, uint8_t p) {
+  AlarmSounder(unsigned long dOn, unsigned long dOff, uint8_t p) {
     DurationOn = dOn;
     DurationOff = dOff;
     pin = p;
     state = LOW;
   }

-  Alarm() { // default
+  AlarmSounder() { // default
     DurationOn = 1000; // 1 second
     DurationOff = 1000; // 1 second
     pin = RELAY1;
@@ -194,7 +202,6 @@

   void PoundTheAlarm() { // shameless Nicki Minaj reference
     if (!state && (millis() - PrevMillis) > DurationOff) {
-    Serial.print("PTA!");
       // turn it on
       digitalWrite(pin, HIGH);
       state = HIGH;
@@ -205,6 +212,12 @@
       state = LOW;
       PrevMillis = millis();
     }
+  }
+
+  void KillTheAlarm() {
+    digitalWrite(pin, LOW);
+    state = LOW;
+    PrevMillis = 0L;
   }
 };

@@ -258,6 +271,8 @@
 };

 DisplayUpdater DU;
+alarm alarms[NALARMS];
+AlarmSounder AS(1000, 500, RELAY1);

 void setup() {
   // serial debug connection
@@ -266,6 +281,10 @@
   pinMode(RELAY1, OUTPUT);
   pinMode(RELAY2, OUTPUT);
   pinMode(RELAY4, OUTPUT);
+  pinMode(KEY_CANCEL, INPUT_PULLUP);
+  pinMode(KEY_DOWN, INPUT_PULLUP);
+  pinMode(KEY_UP, INPUT_PULLUP);
+  pinMode(KEY_OK, INPUT_PULLUP);
   // put your setup code here, to run once:
   Serial.println(F("\nInitialising the LCD ..."));
   lcd.begin(20,4);
@@ -318,11 +337,33 @@
     DateTime cur = parseHeader(ip, 80, WEBSITE, WEBPAGE);
     cur = cur + TimeSpan(0, TZOFFSET, 0, 0);
     rtc.adjust(cur);
-  }
+  }
 }

 void loop() {
   // put your main code here, to run repeatedly:
   now = rtc.now();
   DU.update(lcd, rtc, cc3000);
+  // check alarms
+  uint8_t i;
+  for (i = 0; i < NALARMS; i++) {
+    if (alarms[i].set.unixtime() <= now.unixtime() &&
+        alarms[i].active) {
+      // pound the alarm
+      AS.PoundTheAlarm();
+      alarms[i].sounding = true;
+    }
+  }
+  // do UI stuff
+  if (!digitalRead(KEY_CANCEL)) {
+    Serial.print("Cancelled!");
+    // cancel the alarm
+    for (i = 0; i < NALARMS; i++) {
+      if (alarms[i].sounding) {
+        AS.KillTheAlarm();
+        alarms[i].sounding = false;
+        alarms[i].active = false;
+      }
+    }
+  }
 }


Add DHT22 code to the Arduino code.

Add DHT22 code to the Arduino code.

--- SensorSeed/Arduino/ArduinoFeather/ArduinoFeather.ino
+++ SensorSeed/Arduino/ArduinoFeather/ArduinoFeather.ino
@@ -5,6 +5,8 @@
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_VEML6070.h>
 #include <Adafruit_TSL2561_U.h>
+#include <DHT.h>
+#include <DHT_U.h>
 #include <SPI.h>
 #include <Adafruit_WINC1500.h>
 #include <avr/dtostrf.h>
@@ -16,6 +18,8 @@
 #define VANE_PIN A1
 #define SOLAR_PIN A2
 #define RAIN_GAUGE_PIN 11
+#define DHTPIN            9
+#define DHTTYPE           DHT22

 #define RAIN_FACTOR 0.2794
 #define WIND_FACTOR 2.4
@@ -27,19 +31,20 @@
 #define WINC_EN   2

-volatile unsigned long rain_count=0;
-volatile unsigned long rain_last=0;
-volatile unsigned long anem_count=0;
-volatile unsigned long anem_last=0;
-volatile unsigned long anem_min=0xffffffff;
-static const int vaneValues[] PROGMEM={66,84,92,127,184,244,287,406,461,600,631,702,786,827,889,946};
-static const int vaneDirections[] PROGMEM={1125,675,900,1575,1350,2025,1800,225,450,2475,2250,3375,0,2925,3150,2700};
+volatile unsigned long rain_count = 0;
+volatile unsigned long rain_last = 0;
+volatile unsigned long anem_count = 0;
+volatile unsigned long anem_last = 0;
+volatile unsigned long anem_min = 0xffffffff;
+static const int vaneValues[] PROGMEM = {66, 84, 92, 127, 184, 244, 287, 406, 461, 600, 631, 702, 786, 827, 889, 946};
+static const int vaneDirections[] PROGMEM = {1125, 675, 900, 1575, 1350, 2025, 1800, 225, 450, 2475, 2250, 3375, 0, 2925, 3150, 2700};

 Adafruit_SHT31 sht31 = Adafruit_SHT31();
 Adafruit_BMP085_Unified bmp180 = Adafruit_BMP085_Unified(10085);
 Adafruit_WINC1500 WiFi(WINC_CS, WINC_IRQ, WINC_RST);
 Adafruit_VEML6070 veml6070 = Adafruit_VEML6070();
 Adafruit_TSL2561_Unified tsl = Adafruit_TSL2561_Unified(TSL2561_ADDR_FLOAT, 12345);
+DHT_Unified dht(DHTPIN, DHTTYPE);

 char ssid[] = "wpa.quade.local";      // your network SSID (name)
 char pass[] = "aWq$HDvYE0Zn";   // your network password
@@ -48,7 +53,7 @@
 int status = WL_IDLE_STATUS;
 //IPAddress server(10,0,0,64);
 //#define webpage "/SensorSeed/Sensor/AddSensorData"
-unsigned long lastConnectionTime = 0;
+unsigned long lastConnectionTime = 0;
 const unsigned long postingInterval = 300L * 1000L;

 Adafruit_WINC1500Server server(80);
@@ -56,39 +61,39 @@

 void anemometerClick()
 {
-  long thisTime=millis()-anem_last;
-    anem_count++;
-  anem_last=millis();
-
-    if(thisTime<anem_min)
-    {
-      anem_min=thisTime;
-    }
-
+  long thisTime = millis() - anem_last;
+  anem_count++;
+  anem_last = millis();
+
+  if (thisTime < anem_min)
+  {
+    anem_min = thisTime;
+  }
+
 }

 void rainGageClick()
 {
-    long thisTime=millis()-rain_last;
-    if(thisTime>10)
-    {
-    rain_last=millis();
-      rain_count++;
-    }
+  long thisTime = millis() - rain_last;
+  if (thisTime > 10)
+  {
+    rain_last = millis();
+    rain_count++;
+  }
 }

 void setup() {
 #ifdef WINC_EN
   pinMode(WINC_EN, OUTPUT);
   digitalWrite(WINC_EN, HIGH);
-#endif
+#endif
   Serial.begin(9600);
   Serial.println("Starting...");
   pinMode(VBATPIN, INPUT);
   pinMode(SOLAR_PIN, INPUT);

   initializeSensors();
-
+
   // check for the presence of the shield:
   if (WiFi.status() == WL_NO_SHIELD) {
     Serial.println("WiFi shield not present");
@@ -108,9 +113,9 @@
   }
   server.begin();
   // you're connected now, so print out the status:
-  printWifiStatus();
+  printWifiStatus();
   WiFi.setSleepMode(M2M_PS_H_AUTOMATIC, 1);
-
+
 }

 void loop() {
@@ -149,7 +154,9 @@
           unsigned int dir = getDirection();
           float temperaturebmp180 = getTemperatureBMP180();
           uint16_t veml6070Data = veml6070.readUV();
-          float luxData = getLux();
+          float luxData = getLux();
+          float temperatureDHT22 = getTemperatureDHT22();
+          float humidityDHT22 = getHumidityDHT22();

           client.print("temperature: ");
           client.print(temperature);
@@ -177,20 +184,25 @@
           client.print("\n");
           client.print("solar: ");
           client.print(solar);
-          client.print("\n");
+          client.print("\n");
           client.print("direction: ");
           client.print(dir);
           client.print("\n");
           client.print("temperaturebmp180: ");
           client.print(temperaturebmp180);
-          client.print("\n");
+          client.print("\n");
           client.print("veml6070: ");
           client.print(veml6070Data);
-          client.print("\n");
+          client.print("\n");
           client.print("lux: ");
           client.print(luxData);
-          client.print("\n");
-
+          client.print("\n");
+          client.print("temperatureDHT22: ");
+          client.print(temperatureDHT22);
+          client.print("\n");
+          client.print("humidityDHT22: ");
+          client.print(humidityDHT22);
+          client.print("\n");

           break;
         }
@@ -210,7 +222,7 @@
     // close the connection:
     client.stop();
     Serial.println("client disconnected");
-  }
+  }
 }

 void initializeSensors() {
@@ -225,7 +237,7 @@
   //pinMode(VANE_PWR,OUTPUT);
   //digitalWrite(VANE_PWR,LOW);
   //attachInterrupt(ANEMOMETER_PIN, anemometerClick, FALLING);
-  //attachInterrupt(RAIN_GAUGE_PIN, rainGageClick, FALLING);
+  //attachInterrupt(RAIN_GAUGE_PIN, rainGageClick, FALLING);
   interrupts();
 }

@@ -264,7 +276,7 @@
   String data = String(temp1);
   data.trim();
   delay(5);
-  return data;
+  return data;
 }

 String getRainI2C() {
@@ -283,7 +295,7 @@
   String data = String(temp1);
   data.trim();
   delay(5);
-  return data;
+  return data;
 }

@@ -310,16 +322,28 @@
   return adc;
 }

+float getTemperatureDHT22() {
+  sensors_event_t event;
+  dht.temperature().getEvent(&event);
+  return event.temperature;
+}
+
+float getHumidityDHT22() {
+  sensors_event_t event;
+  dht.humidity().getEvent(&event);
+  return event.relative_humidity;
+}
+
 String getSHT31() {
   float temperature = sht31.readTemperature();
   float humidity = sht31.readHumidity();
   char temp1[10];
-  char temp2[10];
+  char temp2[10];
   dtostrf(temperature, 1, 2, temp1);
   dtostrf(humidity, 1, 2, temp2);
   String data = String(temp1) + "," + String(temp2);
   return data;
-}
+}

 String getBMP180() {
   sensors_event_t event;
@@ -328,7 +352,7 @@
   float temperature;
   bmp180.getTemperature(&temperature);
   float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
-  float altitude = bmp180.pressureToAltitude(seaLevelPressure, event.pressure);
+  float altitude = bmp180.pressureToAltitude(seaLevelPressure, event.pressure);
   char temp1[10];
   char temp2[10];
   char temp3[10];
@@ -345,7 +369,7 @@
   char temp1[10];
   char temp2[10];
   dtostrf(wind, 1, 2, temp1);
-  dtostrf(gust, 1, 2, temp2);
+  dtostrf(gust, 1, 2, temp2);
   String data = String(temp1) + "," + String(temp2);
   return data;
 }
@@ -355,7 +379,7 @@
   char temp1[10];
   dtostrf(rain, 1, 2, temp1);
   String data = String(temp1);
-  return data;
+  return data;
 }

 float getTemperature() {
@@ -379,13 +403,13 @@
 float getPressure() {
   sensors_event_t event;
   bmp180.getEvent(&event);
-  return event.pressure;
+  return event.pressure;
 }

 float getLux() {
   sensors_event_t event;
   tsl.getEvent(&event);
-  return event.light;
+  return event.light;
 }

@@ -398,104 +422,104 @@

 double getUnitWind()
 {
-  unsigned long reading=anem_count;
-  anem_count=0;
-  return (WIND_FACTOR*reading)/(TEST_PAUSE/1000);
-}
-
+  unsigned long reading = anem_count;
+  anem_count = 0;
+  return (WIND_FACTOR * reading) / (TEST_PAUSE / 1000);
+}
+
 double getGust()
 {
-
-  unsigned long reading=anem_min;
-  anem_min=0xffffffff;
-  double time=reading/1000000.0;
-
-  return (1/(reading/1000000.0))*WIND_FACTOR;
-}
-
+
+  unsigned long reading = anem_min;
+  anem_min = 0xffffffff;
+  double time = reading / 1000000.0;
+
+  return (1 / (reading / 1000000.0)) * WIND_FACTOR;
+}
+

 unsigned int getWindDirection() {
-    digitalWrite(VANE_PWR,HIGH);
+  digitalWrite(VANE_PWR, HIGH);
   unsigned int adc;
   adc = averageAnalogRead(VANE_PIN);
-      digitalWrite(VANE_PWR,LOW);
-	/*if (adc < 380) return (113);
-	if (adc < 393) return (68);
-	if (adc < 414) return (90);
-	if (adc < 456) return (158);
-	if (adc < 508) return (135);
-	if (adc < 551) return (203);
-	if (adc < 615) return (180);
-	if (adc < 680) return (23);
-	if (adc < 746) return (45);
-	if (adc < 801) return (248);
-	if (adc < 833) return (225);
-	if (adc < 878) return (338);
-	if (adc < 913) return (0);
-	if (adc < 940) return (293);
-	if (adc < 967) return (315);
-	if (adc < 990) return (270);
-	return (-1); // error, disconnected?  */
-return adc;
+  digitalWrite(VANE_PWR, LOW);
+  /*if (adc < 380) return (113);
+    if (adc < 393) return (68);
+    if (adc < 414) return (90);
+    if (adc < 456) return (158);
+    if (adc < 508) return (135);
+    if (adc < 551) return (203);
+    if (adc < 615) return (180);
+    if (adc < 680) return (23);
+    if (adc < 746) return (45);
+    if (adc < 801) return (248);
+    if (adc < 833) return (225);
+    if (adc < 878) return (338);
+    if (adc < 913) return (0);
+    if (adc < 940) return (293);
+    if (adc < 967) return (315);
+    if (adc < 990) return (270);
+    return (-1); // error, disconnected?  */
+  return adc;
 }

 int averageAnalogRead(int pinToRead)
 {
-	byte numberOfReadings = 8;
-	unsigned int runningValue = 0;
-
-	for(int x = 0 ; x < numberOfReadings ; x++)
-		runningValue += analogRead(pinToRead);
-	runningValue /= numberOfReadings;
-
-	return(runningValue);
+  byte numberOfReadings = 8;
+  unsigned int runningValue = 0;
+
+  for (int x = 0 ; x < numberOfReadings ; x++)
+    runningValue += analogRead(pinToRead);
+  runningValue /= numberOfReadings;
+
+  return (runningValue);
 }

 double getWindVane()
 {
   //analogReference(DEFAULT);
-  digitalWrite(VANE_PWR,HIGH);
+  digitalWrite(VANE_PWR, HIGH);
   delay(100);
-  for(int n=0;n<10;n++)
+  for (int n = 0; n < 10; n++)
   {
     analogRead(VANE_PIN);
   }
-
-  unsigned int reading=analogRead(VANE_PIN);
-  digitalWrite(VANE_PWR,LOW);
-  unsigned int lastDiff=2048;
-
-  for (int n=0;n<16;n++)
+
+  unsigned int reading = analogRead(VANE_PIN);
+  digitalWrite(VANE_PWR, LOW);
+  unsigned int lastDiff = 2048;
+
+  for (int n = 0; n < 16; n++)
   {
-    int diff=reading-pgm_read_word(&vaneValues[n]);
-    diff=abs(diff);
-    if(diff==0)
-       return pgm_read_word(&vaneDirections[n])/10.0;
-
-    if(diff>lastDiff)
+    int diff = reading - pgm_read_word(&vaneValues[n]);
+    diff = abs(diff);
+    if (diff == 0)
+      return pgm_read_word(&vaneDirections[n]) / 10.0;
+
+    if (diff > lastDiff)
     {
-      return pgm_read_word(&vaneDirections[n-1])/10.0;
+      return pgm_read_word(&vaneDirections[n - 1]) / 10.0;
     }
-
-    lastDiff=diff;
- }
-
-  return pgm_read_word(&vaneDirections[15])/10.0;
-
+
+    lastDiff = diff;
+  }
+
+  return pgm_read_word(&vaneDirections[15]) / 10.0;
+
 }

 double getUnitRain()
 {
-
-  unsigned long reading=rain_count;
-  rain_count=0;
-  double unit_rain=reading*RAIN_FACTOR;
-
+
+  unsigned long reading = rain_count;
+  rain_count = 0;
+  double unit_rain = reading * RAIN_FACTOR;
+
   return unit_rain;
 }
-
+

 void printWifiStatus() {
   // print the SSID of the network you're attached to:


No command is now -1

No command is now -1

“No command” is now command -1. 0 is TURN_RIGHT.
Codes can be found here:
http://lirc.10951.n7.nabble.com/robosapien-config-td272.html

--- RobosapienIRCommandSketch/RobosapienIRCommandSketch.ino
+++ RobosapienIRCommandSketch/RobosapienIRCommandSketch.ino
@@ -164,12 +164,12 @@

         Serial.println("Command Number");
         Serial.println(cmdInt);
-        if(cmdInt)
+        if(cmdInt>=0)
         {
           Serial.println("Executing Command");
           sendCommand(cmdInt); //0x88 high5 0x8E fart
           Serial.println("Command Executed, erasing command from firebase");
-          system("curl -X PUT -d '\"0\"' https://blazing-fire-1158.firebaseio.com/robosapien.json");
+          system("curl -X PUT -d '\"-1\"' https://blazing-fire-1158.firebaseio.com/robosapien.json");
           Serial.println("Command erased");
         }
       }


completed gesture control unit without Xbee communication

--- gestureDetection/gestureDetection.ino
+++ gestureDetection/gestureDetection.ino
@@ -34,7 +34,7 @@
  * at fist, index reading 766, middle reading 759, ring reading 796
  * choose middle value 650 to compare and determine if finers are relaxed or curled up
  *
- * data acquisition rate: 781Hz loop speed without any computation, most time spent in writing
+ * data acquisition rate: 755Hz loop speed without any computation, most time spent in writing
  * to accelerometer.
  */

@@ -52,16 +52,18 @@
 int16_t accel_X_reading = 0; //lpf of AcX
 bool indexRelax, middleRelax, ringRelax;
 enum state {idle,forward_drive, backup, left_turn,right_turn};
+state gestureState = idle;
 uint8_t turning_speed; //in forward drive and backup mode, turning_speed is set to 0
                        //in turning state, turning speed is a function of accel reading
+int16_t x; //test variable for interrupt

 void ISR_interrupt(){
   /*upon an interrupt, send catapult throw command*/
   /*right now, we can turn on LED to indicate this event*/
   digitalWrite(13,HIGH);
+  x++;
 }
 void setup(){
-  state gestureState = idle;
   Wire.begin();
   mpu.initialize();
   mpu.dmpInitialize();
@@ -86,8 +88,9 @@
   AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
   AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
   AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
-  accel_X_reading = (int16_t)0.99 * accel_X_reading + (int16_t)0.01 * AcX; //LPF here, check if this works since float is involved
-
+  accel_X_reading = 0.99 * accel_X_reading + 0.01 * AcX; //LPF here, check if this works since float is involved
+  //Serial.print("accel_X_reading is ");Serial.println(accel_X_reading);
+  //Serial.print("number of interrupts is ");Serial.println(x);
   int indexFingerReading = analogRead(A0);
   indexRelax = relax(indexFingerReading);
   int middleFingerReading = analogRead(A1);
@@ -100,25 +103,29 @@
   if (indexRelax && middleRelax && ringRelax && abs(accel_X_reading) < 500){
     //now we use AcX as the only reference to turning angle,
     //might be more useful to calculate angle instead but this is what we have now
-    state gestureState = idle;
+    gestureState = idle;
     turning_speed = 0;
   }else if (!indexRelax && !middleRelax && !ringRelax){
-    state gestureState = backup;
+    gestureState = backup;
     turning_speed = 0;
   }else if (indexRelax && middleRelax && !ringRelax){
-    state gestureState = forward_drive;
+    gestureState = forward_drive;
     turning_speed = 0;
   }else if (accel_X_reading > 500){
-    state gestureState = left_turn;
+    gestureState = left_turn;
     turning_speed = (int8_t)accel_X_reading;//extract top 8 bits of accel_X_reading
   }else if (accel_X_reading < -500){
-    state gestureState = right_turn;
+    gestureState = right_turn;
     turning_speed = (int8_t)accel_X_reading;//extract top 8 bits of accel_X_reading
   }

+  //Serial.print("state is "); Serial.println(gestureState);
+  //Serial.print("turningSpeed is "); Serial.println(turning_speed);
+  //delay(333);
   /*Xbee send packets to inform*/
   //send_info(gestureState,turningSpeed);
-
+
+
   /*
   Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
   GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)


Update SparkCore.ino

Update SparkCore.ino

--- firmware/examples/Transports/SparkCore/SparkCore.ino
+++ firmware/examples/Transports/SparkCore/SparkCore.ino
@@ -35,7 +35,7 @@
         // Note:
         //   We allow 1 tweet per minute for now.
         //   Twitter doesn't allow identical subsequent messages.
-        Blynk.tweet("My Particle project is tweeting using @blynk_app and this is awesome!\n @Particle #IoT #blynk");
+        Blynk.tweet("My Particle project is tweeting using @blynk_app and it’s awesome!\n @Particle #IoT #blynk");

         // Pushing notification to the app!
         // Note:


- indentation fixed

- indentation fixed

--- GeheimTuer.ino
+++ GeheimTuer.ino
@@ -808,38 +808,38 @@
     }

     if (!fanOverride) {
-    if (dsRead) {
-      if (maxTemp <= (tempMin - tempHyst)) {
-        if (fanPWM > 0) {
-          debug(currentMillis, F("Fan off\r\n"));
-          fanPWM = 0;
-        }
-      } else if (maxTemp >= tempMax) {
+      if (dsRead) {
+        if (maxTemp <= (tempMin - tempHyst)) {
+          if (fanPWM > 0) {
+            debug(currentMillis, F("Fan off\r\n"));
+            fanPWM = 0;
+          }
+        } else if (maxTemp >= tempMax) {
+          if (fanPWM < 255) {
+            debug(currentMillis, F("Fan full\r\n"));
+            fanPWM = 255;
+          }
+        } else if (maxTemp >= tempMin) {
+          byte newPWM = map(maxTemp, tempMin, tempMax, fanMin, fanMax);
+          if (newPWM != fanPWM) {
+            fanPWM = newPWM;
+            debug(currentMillis, F("Fan PWM: "));
+            Serial.print(fanPWM);
+            Serial.print(F(" ("));
+            Serial.print(maxTemp);
+            Serial.print(F(")\r\n"));
+          }
+        } else if (fanPWM > 0) {
+          fanPWM = fanMin;
+        }
+      } else {
+        // no temperature sensor responded
+        // run fan at full speed
         if (fanPWM < 255) {
-          debug(currentMillis, F("Fan full\r\n"));
+          debug(currentMillis, F("No temperature reading! - Fan full\r\n"));
           fanPWM = 255;
         }
-      } else if (maxTemp >= tempMin) {
-        byte newPWM = map(maxTemp, tempMin, tempMax, fanMin, fanMax);
-        if (newPWM != fanPWM) {
-          fanPWM = newPWM;
-          debug(currentMillis, F("Fan PWM: "));
-          Serial.print(fanPWM);
-          Serial.print(F(" ("));
-          Serial.print(maxTemp);
-          Serial.print(F(")\r\n"));
-        }
-      } else if (fanPWM > 0) {
-        fanPWM = fanMin;
-      }
-    } else {
-      // no temperature sensor responded
-      // run fan at full speed
-      if (fanPWM < 255) {
-        debug(currentMillis, F("No temperature reading! - Fan full\r\n"));
-        fanPWM = 255;
-      }
-    }
+      }

       analogWrite(fanPWMPin, fanPWM);
     }


Update for 7" panels

Update for 7" panels

--- examples/selftest.ino
+++ examples/selftest.ino
@@ -33,7 +33,7 @@
 void setup()
 {
   Serial.begin(UART_SPEED);
-  Serial.println("---- GAMEDUINO 2 SELFTEST ----");
+  Serial.println("---- GAMEDUINO 2/3 SELFTEST ----");
 }

 static void ramp(int y, uint32_t color)
@@ -49,24 +49,25 @@

   GD.get_inputs();
   GD.Clear();
-  GD.cmd_text(240, 12, 28, OPT_CENTER,
+  GD.cmd_text(GD.w / 2, 12, 28, OPT_CENTER,
     GD3 ? "Gameduino3 Self test" :
           "Gameduino2 Self test");
   GD.cmd_text(GD.w - 2, 12, 27, OPT_CENTERY | OPT_RIGHTX,
     GD2_VERSION);
   int y;

-  y = 50;
+  y = PIXELS(50);
   GD.Begin(POINTS);
   for (int i = 0; i < 6; i++) {
     byte l = 4 << i;
+    int x = map(1 + i, 0, 7, 0, PIXELS(GD.w));
     GD.PointSize(280);
     GD.ColorRGB(0xffffff);
-    GD.Vertex2ii(68 * (i + 1), y, 0, 0);
+    GD.Vertex2f(x, y);

     GD.PointSize(240);
     GD.ColorRGB(l, l, l);
-    GD.Vertex2ii(68 * (i + 1), y, 0, 0);
+    GD.Vertex2f(x, y);
   }
   y += 30;

@@ -84,7 +85,7 @@
     GD.ColorRGB(0x808000);
   else
     GD.ColorRGB(pass ? 0x40ff40 : 0xff4040);
-  GD.cmd_text(120, 180, 31, OPT_CENTERY, message);
+  GD.cmd_text(GD.w / 2, 180, 31, OPT_CENTER, message);

   GD.ColorRGB(0xffffff);
   GD.Begin(LINES);
@@ -287,8 +288,6 @@
 static byte test_touch(void)
 {
   if (!GD3) {
-    GD.Clear();
-    GD.cmd_text(240, 100, 30, OPT_CENTERX, "please tap on the dot");
     GD.self_calibrate();
     // write the new calibration back to EEPROM

@@ -610,36 +609,42 @@
   i2c_stop();
 }

-static void load_flash()
-{
-  GD.begin(0);
-
-  if (GD3) {
-    uint8_t stage[128];
-    memcpy(stage, GD3_43__init, 128);
+static void load_flash(uint8_t *config)
+{
+    byte b[128];

     i2c_begin();
-
-    GD.Clear();
-    GD.cmd_text(240, 100, 30, OPT_CENTERX, "please tap on the dot");
-    GD.self_calibrate();
-    GD.finish();
-
-    for (int i = 0; i < 24; i++)
-      stage[CALIBRATION_OFFSET + i] = GD.rd(REG_TOUCH_TRANSFORM_A + i);
-
-    ram_write(stage);
-    byte b[128];
+    ram_write(config);
     ram_get(b);
-    Serial.print("compare ");
-    int diff = memcmp(stage, b, 128);
+    int diff = memcmp(config, b, 128);
     if (diff != 0) {
+      Serial.println("Flash fault");
       GD.Clear();
-      GD.cmd_text(240, 100, 30, OPT_CENTERX, "FLASH fault");
+      GD.cmd_text(GD.w / 2, GD.h / 2, 30, OPT_CENTERX, "Flash fault");
       GD.swap();
       for (;;);
     }
-    Serial.println(diff);
+    Serial.println("Flash verified OK");
+    GD.begin(0);
+}
+
+static void setup_flash()
+{
+  GD.begin(0);
+
+  if (GD3) {
+    uint8_t stage[128];
+    memcpy(stage, GD3_7__init, 128);
+
+    load_flash(stage);
+
+    // GD.self_calibrate();
+    GD.finish();
+
+    for (int i = 0; i < 24; i++)
+      stage[CALIBRATION_OFFSET + i] = GD.rd(REG_TOUCH_TRANSFORM_A + i);
+
+    load_flash(stage);
   }
 }

@@ -647,7 +652,7 @@
 {
   if (EEPROM.read(0) == 0x7c)
     EEPROM.write(0, 0xff);
-  load_flash();
+  setup_flash();
   GD.begin(0);

   x = y = 0;


finally handling output of 64-bit uints properly, I hope

--- examples/PCA9548A_Test/PCA9548A_Test.ino
+++ examples/PCA9548A_Test/PCA9548A_Test.ino
@@ -56,6 +56,8 @@
 uint8_t inbyte = 0;

 uint32_t old_total_error_count = 0;
+
+

 /* ========== SETUP ========== */
 void setup(void)
@@ -94,6 +96,16 @@
   Serial.printf("Using i2c_t3 I2C library for Teensy\r\n");
 #endif

+  uint64_t uint64_test = 0;
+  uint64_test = UINT64_MAX;
+  // Serial.println(uint64_test);
+  uint64_test++;
+  // Serial.println(uint64_test);
+  Serial.printf("More than 32 bits? %llu, 0x%llX\r\n", uint64_test, uint64_test);
+  Serial.printf ("UINT32_MAX=0x%X, UINT64_MAX=0x%llX\r\n", UINT32_MAX, UINT64_MAX);
+  Serial.printf ("UINT64_MAX=0x%llX, UINT32_MAX=0x%X\r\n", UINT64_MAX, UINT32_MAX);
+  Serial.printf ("UINT32_MAX=%u, UINT64_MAX=%llu\r\n", UINT32_MAX, UINT64_MAX);
+  if (0xFFFFFFFFFFFFFFFE < ULLONG_MAX) Serial.println ("Test of ULLONG_MAX seems to pass");
   if (0xFFFFFFFFFFFFFFFE < UINT64_MAX) Serial.println ("Test of UINT64_MAX seems to pass");

   // start PCA9548A library
@@ -290,10 +302,10 @@
     {
       minute_ten_tick = false;
       new_errors = false;
-      Serial.printf("\r\net:%u  Good:%u  %u/sec", total_elapsed_seconds, PCA9548A_70.error.successful_count, PCA9548A_70.error.successful_count/(total_elapsed_seconds-startuptime));
+      Serial.printf("\r\net:%u  Good:%llu  %llu/sec", total_elapsed_seconds, PCA9548A_70.error.successful_count, PCA9548A_70.error.successful_count/(uint64_t)(total_elapsed_seconds-startuptime));
       if (PCA9548A_70.error.total_error_count)
         {
-          Serial.printf("  bad:%u", PCA9548A_70.error.total_error_count);
+          Serial.printf("  bad:%llu", PCA9548A_70.error.total_error_count);
 #if defined I2C_AUTO_RETRY
     Serial.printf("  resetBus: %u", Wire.resetBusCountRead());
 #endif