new code

Code/feather_main_personaldemo.ino

+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+#include "Adafruit_VL53L0X.h"
+#include "ArnholdMesh.h"
+#include <Arduino.h>
+#include <string>
+#include <vector>
+#include "Camera.h"
+#include <PID_v1.h>
+
+#define   STATION_SSID     "lemur"
+#define   STATION_PASSWORD ""
+#define DDRIVE_MIN -1 //The minimum value x or y can be.
+#define DDRIVE_MAX 1  //The maximum value x or y can be.
+#define MOTOR_MIN_PWM -255 //The minimum value the motor output can be.
+#define MOTOR_MAX_PWM 255 //The maximum value the motor output can be.
+double Setpointx, Inputx, Outputx;
+double Setpointy, Inputy, Outputy;
+
+//Identify all the global constants that will be used by the robot
+const int BAUD_RATE = 115200;
+const int MAX_SPEED = 255;
+const double RESOLUTION_W = 320.0;
+const double RESOLUTION_H = 240.0;
+const int SEEKING_SPEED = 60;
+const int TIME_STEP = 1;
+const int ENDDEMO_TAG = 0;
+
+//Identify all the variables that will be used by the robot
+int findcolor = 1; //based on the same code from openMV. Determine the color you want to find
+int NOMINAL_PERCENTAGE = 80; //in percent
+char newPTCL = '1';
+int altitude = 0;
+int desiredAltitude = 0;
+int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
+
+////Identify the variables that will be communicated through DASH
+//bool manualMode = true; 
+//int verticalSpeed = 0; //0 to 255
+//bool verticalDirection = true;
+//float horiAnalog = 0; //between -1 to 1
+//float vertAnalog = 0;
+
+//Create the interface that will be used by the camera
+openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+
+// Create the motor shield object with the default I2C address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
+Adafruit_DCMotor *motorVertical = AFMS.getMotor(1);
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(2);
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
+
+//Create the LIDAR object
+//Adafruit_VL53L0X lidar = Adafruit_VL53L0X();
+
+//ArnholdMesh thisNode;
+Camera cam(&interface);
+
+//Specify the links and initial tuning parameters
+double Kp=1, Ki=0.05, Kd=0.25;
+PID PID_x(&Inputx, &Outputx, &Setpointx, Kp, Ki, Kd, DIRECT);
+PID PID_y(&Inputy, &Outputy, &Setpointy, Kp, Ki, Kd, DIRECT);
+
+void setup() {
+  Serial.begin(BAUD_RATE);
+  translateCodetoThreshold(findcolor);
+  Wire.begin();
+  AFMS.begin();  // create with the default frequency 1.6KHz
+  interface.begin();
+  
+  Setpointx = 160.0;
+  Setpointy = 120.0;
+  PID_y.SetOutputLimits(-255, 255);
+  PID_x.SetOutputLimits(-255, 255);
+  PID_x.SetMode(AUTOMATIC);
+  PID_y.SetMode(AUTOMATIC);
+  
+  //thisNode.setStationInfo(STATION_SSID, STATION_PASSWORD);
+  //thisNode.init();
+//  if (!lidar.begin()) {
+//    while(1){
+//      //NEED: Light up red LED
+//    }
+//  }
+}
+
+void loop() {
+    int x = 0;
+    int y = 0;
+    if(cam.exe_color_detection_biggestblob(threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], threshold[6], x, y)){
+      Serial.print("x value: ");
+      Serial.println(x);
+      Serial.print("y value: ");
+      Serial.println(y);
+      Inputx = x/1.00;
+      Inputy = y/1.00;
+      PID_x.Compute();
+      PID_y.Compute();
+      Serial.print("the x output is: ");
+      Serial.println(Outputx);
+      Serial.print("the y output is: ");
+      Serial.println(Outputy);
+    } else {
+      Serial.println("not detected");
+    }
+  
+//  //Check to see if we want to end the demo
+//  int xtemp = 0;
+//  int ytemp = 0;
+//  int angletemp = 0;
+//  if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
+//    motorVertical->setSpeed(0);
+//    motorVertical->run(FORWARD);
+//    // turn on motor
+//    motorVertical->run(RELEASE);
+//    motorLeft->setSpeed(0);
+//    motorLeft->run(FORWARD);
+//    motorRight->setSpeed(0);
+//    motorRight->run(BACKWARD);
+//    while(1){
+//      Serial.println("end of demo");
+//    }
+//  }
+//  
+//  int x = 0;
+//  int y = 0;
+//  if(cam.exe_color_detection_biggestblob(threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], threshold[6], x, y)){
+//    Serial.println("blob detected");
+//    Input = x;
+//    myPID.Compute();
+//    Serial.print("the output is: ");
+//    Serial.println(Output);
+//    int dy = y - RESOLUTION_H/2.0;
+//    int dx = x - RESOLUTION_W/2.0;
+//    int y_speed = abs(dy/(RESOLUTION_H/2.0))*MAX_SPEED*NOMINAL_PERCENTAGE/100.0; 
+//    int x_speed = abs(dx/(RESOLUTION_W/2.0))*MAX_SPEED*NOMINAL_PERCENTAGE/100.0; 
+//    int lockon_speed = MAX_SPEED*NOMINAL_PERCENTAGE/200.0;
+//        Serial.print("y value: ");
+//        Serial.println(y);
+//        Serial.print("velocity: ");
+//        Serial.println(y_speed);
+//    if (dy < 0) {
+//      motorVertical->setSpeed(100 + y_speed);
+//      motorVertical->run(BACKWARD);
+//    } else {
+//      motorVertical->setSpeed(100 + y_speed);
+//      motorVertical->run(FORWARD);
+//    }
+//    if (dx < 0) { //
+//      motorRight->setSpeed(SEEKING_SPEED/2); //(SEEKING_SPEED);
+//      motorRight->run(BACKWARD);
+//      motorLeft->setSpeed(min(255, (SEEKING_SPEED/2) + x_speed)); //this need to be higher
+//      motorLeft->run(BACKWARD);
+//    } else {
+//      motorRight->setSpeed(min(255,(SEEKING_SPEED/2) + x_speed)); //this need to be higher
+//      motorRight->run(BACKWARD);
+//      motorLeft->setSpeed(SEEKING_SPEED/2); //(SEEKING_SPEED);
+//      motorLeft->run(BACKWARD);
+//    }
+//  } else { //seeking algorithm
+//      Serial.println("seeking...");
+//      motorVertical->setSpeed(0);
+//      motorVertical->run(FORWARD);
+//      // turn on motor
+//      motorVertical->run(RELEASE);
+//      Serial.println(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+//      motorLeft->setSpeed(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+//      motorLeft->run(FORWARD); 
+//      motorRight->setSpeed(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+//      motorRight->run(BACKWARD);
+//  }
+}
+
+void translateCodetoThreshold(int code){
+  switch(code){
+    case 1: //green
+      threshold[1] = 30;
+      threshold[2] = 100;
+      threshold[3] = -68;
+      threshold[4] = -13;
+      threshold[5] = 30;
+      threshold[6] = 127;
+    break;
+    case 2: //blue
+      threshold[1] = 30;
+      threshold[2] = 100;
+      threshold[3] = -108;
+      threshold[4] = -9;
+      threshold[5] = 0;
+      threshold[6] = -42;
+    break;
+    case 5: //red
+      threshold[1] = 30;
+      threshold[2] = 100;
+      threshold[3] = 127;
+      threshold[4] = 23;
+      threshold[5] = 127;
+      threshold[6] = -13;
+  }
+}