diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.cpp
deleted file mode 100644
index 2eff7f6c9d9831e22e3d071877859cc18ab0a116..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-#include "Camera.h"
-#include <Wire.h>
-#include <Arduino.h>
-#include <openmvrpc.h>
-
-Camera::Camera(openmv::rpc_i2c_master *intface){
- interface = intface;
-}
-
-void Camera::exe_face_detection(){
- struct { uint16_t x, y, w, h; } face_detection_result;
- if (interface->call_no_args(F("face_detection"), &face_detection_result, sizeof(face_detection_result))) {
- Serial.print(F("Largest Face Detected [x="));
- Serial.print(face_detection_result.x);
- Serial.print(F(", y="));
- Serial.print(face_detection_result.y);
- Serial.print(F(", w="));
- Serial.print(face_detection_result.w);
- Serial.print(F(", h="));
- Serial.print(face_detection_result.h);
- Serial.println(F("]"));
- }
-}
-
-bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
- struct { uint16_t cx, cy, rot; } result;
- if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
- }
- if (result.cx == 0 && result.cy == 0 && result.rot == 0){
- return false;
- } else {
- *x_temp = result.cx;
- *y_temp = result.cy;
- *angle_temp = result.rot;
- return true;
- }
-}
-
-bool Camera::exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max){
- int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
- short buff[128 + 1] = {};
- if (interface->call(F("color_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
- int i = 0;
- while (buff[i] != '\0' && i<100) {
- Serial.print(buff[i]);
- i++;
- }
- Serial.println("");
- }
- if (buff[0] == 0){ //no blob detected
- return false;
- } else {
- return true;
- }
-}
-
-bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y){
- int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
- struct { uint16_t cx, cy; } color_detection_result;
- if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) {
-
- }
- x = color_detection_result.cx;
- y = color_detection_result.cy;
- if (x == 0 && y == 0){
- return false;
- } else {
- return true;
- }
-}
-
-void Camera::exe_led_detection(){
- //int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
- int8_t color_thresholds[6] = {26, 100, -108, -9, 0, -42};
- //struct { uint16_t cx, cy; } color_detection_result;
- short buff[128 + 1] = {};
- if (interface->call(F("LED_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
- int i = 0;
- Serial.println("LED detected");
- while (buff[i] != '\0' && i<100) {
- Serial.print(buff[i]);
- i++;
- }
- Serial.println("");
- }
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.h
deleted file mode 100644
index 55a9d4e29f73a009260340edd03e1cc85a035c09..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/Camera.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef CAMERA_H
-#define CAMERA_H
-#include <openmvrpc.h>
-
-class Camera
-{
- private:
- openmv::rpc_i2c_master *interface;
-
- public:
- Camera(openmv::rpc_i2c_master *intface);
- void exe_face_detection(); // Face should be about 2ft away.
- bool exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp);
- bool exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max);
- bool exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y);
- void exe_led_detection();
-
-};
-#endif
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.cpp
deleted file mode 100644
index eb4ee53b1e31f36bf48e1ea4cf8ecff909fb74c3..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-#include "LedPanel.h"
-
-LedPanel::LedPanel(int pixelnum, int pixelpin){
- led_num = pixelnum;
- //pixels = Adafruit_NeoPixel(pixelnum, pixelpin, NEO_GRB + NEO_KHZ800);
-}
-
-void LedPanel::fullPanelLight(int r, int g, int b){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, r,g,b);
- }
- pixels.show();
-}
-
-void LedPanel::beginPanel(){
- pixels.begin();
-}
-
-void LedPanel::resetPanel(){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, 0,0,0);
- }
- pixels.show();
-}
-
-void LedPanel::topLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::topRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::singleLED(int num, int r, int g, int b) {
- pixels.setPixelColor(num, r, g, b);
- pixels.show();
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.h
deleted file mode 100644
index 41721cc9dd7750951e5345169c2eb604d549f704..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/LedPanel.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef LEDPANEL_H
-#define LEDPANEL_H
-
-#include <Adafruit_NeoPixel.h>
-#define NUMPIXELS 32
-#define PIN_PIXELS 32 // Use pin 32 for NeoPixels
-const int HORIZONTAL_SIZE = 8;
-const int VERTICAL_SIZE = 4;
-
-class LedPanel {
- private:
- int led_num;
- Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN_PIXELS, NEO_GRB + NEO_KHZ800);
-
- public:
- LedPanel(int pixelnum, int pixelpin);
- void fullPanelLight(int r, int b, int g);
- void resetPanel();
- void topLeftQuadrant(int r, int g, int b);
- void bottomLeftQuadrant(int r, int g, int b);
- void topRightQuadrant(int r, int g, int b);
- void bottomRightQuadrant(int r, int g, int b);
- void beginPanel();
- void singleLED(int num, int r, int g, int b);
-
-};
-
-#endif
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/feather_main_test1_PID.ino b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/feather_main_test1_PID.ino
deleted file mode 100644
index c337355ffa2843690a617cf2c5b7fe5fbdb3349e..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/feather_main_test1_PID.ino
+++ /dev/null
@@ -1,391 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_MotorShield.h>
-
-#include "LedPanel.h"
-#include <Arduino.h>
-#include <string>
-#include <vector>
-#include "Camera.h"
-#include <PID_v1.h>
-#include "utilities.h"
-
-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 int SEEKING_SPEED = 70;
-const double RESOLUTION_W = 320.0;
-const double RESOLUTION_H = 240.0;
-const int ENDDEMO_TAG = 0;
-const uint32_t THISNODE_ID = 88789821;
-
-//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
-char newPTCL = '1';
-int pau = 0;
-int displayTracking = 0;
-int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 30; //125;
-
-//The debug LEDs that we will be using. Description at:
-const int DEBUG_STATE = 31;
-const int DEBUG_KP = 30;
-const int DEBUG_KI = 29;
-const int DEBUG_KD = 28;
-const int DEBUG_BASESPEED = 27;
-const int DEBUG_THRESHOLD_MIN = 26;
-const int DEBUG_THRESHOLD_MAX = 25;
-const int DEBUG_VERTICALSPEED = 17;
-const int DEBUG_RSPEED = 16;
-const int DEBUG_LSPEED = 24;
-
-//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_L = AFMS.getMotor(2);
-Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(1);
-Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
-Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
-
-
-Camera cam(&interface);
-LedPanel panel(NUMPIXELS,PIN_PIXELS);
-
-
-//Specify the links and initial tuning parameters
-double Kpx=2, Kix=0.05, Kdx=0.25;
-double Kpy=2, Kiy=0.1, Kdy=0.25;
-PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
-PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
-
-void setup() {
- Serial.begin(BAUD_RATE);
- translateCodetoThreshold(findcolor);
- Wire.begin();
- AFMS.begin();
- interface.begin(); //communication between ESP and OpenMV
- panel.beginPanel();
-
- Setpointx = 160.0;
- Setpointy = 120.0; //the values that the PID will try to reach
- PID_y.SetOutputLimits(-255, 255); //up positive
- PID_x.SetOutputLimits(-255, 255); //left positive
- PID_x.SetMode(AUTOMATIC);
- PID_y.SetMode(AUTOMATIC);
-}
-
-void loop() {
- panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
- while(pau == 1){
- int zero = 0;
- moveVertical(zero);
- moveHorizontal(zero,zero);
- panel.singleLED(DEBUG_STATE, 10, 10, 0);
- Serial.println("pause");
- }
-
- //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)){
- int zero = 0;
- moveVertical(zero);
- moveHorizontal(zero,zero);
- panel.singleLED(DEBUG_STATE, 0, 0, 10);
- while(1){
- Serial.println("end of demo");
- }
- }*/
-
- //if the demo is still ongoing, check to see if there is a desired-color blob
- int x = 0;
- int y = 0;
- int dist = 0;
-
- panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
- int det_tag = 1;
- if(cam.exe_apriltag_detection(det_tag, &x, &y, &dist)){
- if (displayTracking > 0){
- displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
- }
- panel.singleLED(DEBUG_STATE, 0, 10, 0);
- Serial.println("blob detected");
- Serial.print("x value: ");
- Serial.println(x);
- Serial.print("y value: ");
- Serial.println(y);
- Serial.print("dist value: ");
- Serial.println(dist);
- Inputx = x/1.00;
- Inputy = y/1.00;
- PID_x.Compute();
- PID_y.Compute();
-
- Serial.println(Outputy);
- Serial.println(Outputx);
-
- //actuate the vertical motor
- moveVertical(Outputy);
- moveHorizontal(Outputx, BASE_SPEED);
-
- } else { //seeking algorithm
- //panel.resetPanel();
- panel.singleLED(DEBUG_STATE, 10, 10, 10);
- Serial.println("seeking...");
- int zero = 0;
- moveVertical(zero);
- moveHorizontal(5, zero);
- }
-}
-
-//vel value should be between -255 to 255 with positive values moving the blimp
-//upward.
-void moveVertical(int vel){
- if (vel > 0) { //up
- panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0);
- motorVertical_L->setSpeed(abs((int) vel));
- motorVertical_R->setSpeed(abs((int) vel));
- motorVertical_L->run(BACKWARD);
- motorVertical_R->run(BACKWARD);
- } else if(vel < 0) { //down
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
- motorVertical_L->setSpeed(abs((int) Outputy));
- motorVertical_R->setSpeed(abs((int) Outputy));
- motorVertical_L->run(FORWARD);
- motorVertical_R->run(FORWARD);
- } else {
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
- motorVertical_L->setSpeed(0);
- motorVertical_R->setSpeed(0);
- }
-}
-
-void moveHorizontal(int vel_hori,int base_speed){
- int lspeed = -1*vel_hori + base_speed;
- int rspeed = vel_hori + base_speed;
-
- if (rspeed > 0){
- motorLeft->run(FORWARD);
- } else {
- motorLeft->run(FORWARD);
- }
-
- if (lspeed > 0){
- motorRight->run(BACKWARD);
- } else {
- motorRight->run(FORWARD);
- }
- displaySpeed(lspeed, rspeed);
- motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed)));
- motorRight->setSpeed(min(MAX_SPEED, abs(lspeed)));
-}
-
-void displaySpeed(int lspeed, int rspeed){
- //Serial.println("display speed");
- if (lspeed < 0){
- panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed));
- } else {
- panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0);
- }
-
- if (rspeed < 0){
- panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed));
- } else {
- panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0);
- }
-}
-
-//When using the camera to detect objects such as colored blobs or april tags. This function is
-//useful when only a single object is the target. The approximate position will be marked with an
-//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be
-//called assuming that you have created the LED panel object, which in this code is named "panel".
-//If the LED panel is called something else, just edit the code here
-void displayTrackedObject(int cx, int cy, int w_res, int h_res){
- int lednum = 0;
- int vertshift = 0;
- panel.resetPanel();
- lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
- vertshift = cy/(h_res/4);
- lednum = lednum + 8*vertshift;
- panel.singleLED(lednum, 10, 10 , 10);
-}
-
-
-//Interpret a message to change what color the camera is detecting
-void setColor(String msg) {
- if (msg.length() < 3){
- int val = msg.toInt();
- translateCodetoThreshold(val);
- Serial.println("code detected");
- } else {
- Serial.println("new threshold detected");
- setColorThreshold(msg, threshold, 6);
- }
- for (int j = 0; j < 6; j++){
- Serial.print(threshold[j]);
- Serial.print(" ");
- }
- Serial.println("");
-}
-
-//in case we don't want to maually set each LAB threshold, we can send over an int to
-//use preset threshold values instead
-void translateCodetoThreshold(int code){
- switch(code){
- case 1: //green old = (30, 100, -68, -13, 30, 127)
- //(30,100,-68,2,6,127) - detect the yellow wall as well
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = -93;
- threshold[3] = -5;
- threshold[4] = 13;
- threshold[5] = 127;
- break;
- case 2: //blue
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = -108;
- threshold[3] = -9;
- threshold[4] = 0;
- threshold[5] = -42;
- break;
- case 5: //red (30, 100, 127, 41, 127, 13)
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = 127;
- threshold[3] = 41;
- threshold[4] = 127;
- threshold[5] = 13;
- }
-}
-
-
-//threshold array must have at least six elements. This function helps
-//translating a message with threshold values to ints. Example msg that would
-//be received by this function are "1 2 3 4 5 6" or "-100 70 9 -9 -50 128".
-//NOTE: - the threshold value should be between -128 to 128
-// - the message should not include the command character used by the mesh
-// - suggested command character: 'C'[olor]
-void setColorThreshold(String msg, int8_t thres[], int arraySize){
- int len = msg.length();
- int temp = 0;
- int startpoint = 0;
- for (int i = 0; i < len; i++){
- if (msg[i] == ' '){
- thres[temp] = msg.substring(startpoint,i).toInt();
- startpoint = i + 1;
- temp++;
- }
- if (temp == 5){
- thres[temp] = msg.substring(startpoint).toInt();
- break;
- }
- }
-}
-
-//This function translate a string of message into the constants for a PID controller. Ignoring the
-//first command character, user can input any one, two, or all three parameter to be changed by identifying
-//the new parameter with the capital letter of that specific parameter, each separated with a space.
-//Some of the valid msg examples are:
-// - "P0.02"
-// - "D1.23"
-// - "P0.14 D9.5"
-// - "P0.1 I0.1 D0.1"
-//
-//NOTE:
-// - the parameter doesn't have to be in order. You can do DI or IP in whichever order as long as it follows a valid double value
-// - while the function works if you passed a negative double, the PID controller will not and will produce error.
-// - suggested command character: 'T'[uning]
-void setPIDConstants(String msg, double &p_constant, double &i_constant, double &d_constant){
- double new_p = Kpx;
- double new_i = Kix;
- double new_d = Kdx;
-
- int len = msg.length();
- int startpoint = 0;
- int endpoint = 0;
- for (int i = 0; i < len; i++){
- if (msg[i] == 'P'){
- startpoint = i + 1;
- for (int j = i + 1; j < len; j++){
- if (msg[j] == ' '){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- new_p = msg.substring(startpoint, endpoint).toDouble();
- }
- }
-
- if (msg[i] == 'I'){
- startpoint = i + 1;
- for (int j = i + 1; j < len; j++){
- if (msg[j] == ' '){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- //i_constant = msg.substring(startpoint, endpoint).toDouble();
- new_i = msg.substring(startpoint, endpoint).toDouble();
- }
- }
-
- if (msg[i] == 'D'){
- startpoint = i + 1;
- for (int j = i + 1; j <= len; j++){
- if (msg[j] == ' ' || msg[j] == '\0'){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- //d_constant = msg.substring(startpoint, endpoint).toDouble();
- new_d = msg.substring(startpoint, endpoint).toDouble();
- }
- }
- }
-
- //DEBUGGING SECTION
- unsigned long t = millis();
- debugPIDConstants(DEBUG_KP, p_constant, new_p);
- debugPIDConstants(DEBUG_KI, i_constant, new_i);
- debugPIDConstants(DEBUG_KD, d_constant, new_d);
-
- p_constant = new_p;
- i_constant = new_i;
- d_constant = new_d;
-
- while(millis() < t+2000); //debugging LED will light up for 2 seconds
- panel.singleLED(DEBUG_KP, 0, 0, 0);
- panel.singleLED(DEBUG_KI, 0, 0, 0);
- panel.singleLED(DEBUG_KD, 0, 0, 0);
-}
-
-void debugPIDConstants(int lednum, double oldval, double newval){
- int r, g, b;
- if (newval > oldval){
- panel.singleLED(lednum, 0, 10, 0);
- } else if (newval < oldval){
- panel.singleLED(lednum, 10, 0, 0);
- } else { //equal
- panel.singleLED(lednum, 10, 10, 10);
- }
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.cpp
deleted file mode 100644
index 7a90e33a9ea7f73c7ad525b0919f9c69b4928fda..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-#include <math.h>
-
-//Source: http://www.easyrgb.com/index.php?X=MATH&H=01#text1
-void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B )
-{
- float var_Y = ( l_s + 16. ) / 116.;
- float var_X = a_s / 500. + var_Y;
- float var_Z = var_Y - b_s / 200.;
-
- if ( pow(var_Y,3) > 0.008856 ) var_Y = pow(var_Y,3);
- else var_Y = ( var_Y - 16. / 116. ) / 7.787;
- if ( pow(var_X,3) > 0.008856 ) var_X = pow(var_X,3);
- else var_X = ( var_X - 16. / 116. ) / 7.787;
- if ( pow(var_Z,3) > 0.008856 ) var_Z = pow(var_Z,3);
- else var_Z = ( var_Z - 16. / 116. ) / 7.787;
-
- float X = 95.047 * var_X ; //ref_X = 95.047 Observer= 2°, Illuminant= D65
- float Y = 100.000 * var_Y ; //ref_Y = 100.000
- float Z = 108.883 * var_Z ; //ref_Z = 108.883
-
-
- var_X = X / 100. ; //X from 0 to 95.047 (Observer = 2°, Illuminant = D65)
- var_Y = Y / 100. ; //Y from 0 to 100.000
- var_Z = Z / 100. ; //Z from 0 to 108.883
-
- float var_R = var_X * 3.2406 + var_Y * -1.5372 + var_Z * -0.4986;
- float var_G = var_X * -0.9689 + var_Y * 1.8758 + var_Z * 0.0415;
- float var_B = var_X * 0.0557 + var_Y * -0.2040 + var_Z * 1.0570;
-
- if ( var_R > 0.0031308 ) var_R = 1.055 * pow(var_R , ( 1 / 2.4 )) - 0.055;
- else var_R = 12.92 * var_R;
- if ( var_G > 0.0031308 ) var_G = 1.055 * pow(var_G , ( 1 / 2.4 ) ) - 0.055;
- else var_G = 12.92 * var_G;
- if ( var_B > 0.0031308 ) var_B = 1.055 * pow( var_B , ( 1 / 2.4 ) ) - 0.055;
- else var_B = 12.92 * var_B;
-
- R = var_R * 255.;
- G = var_G * 255.;
- B = var_B * 255.;
-
-}
-
-
-
-
-
-//old codes
-
-// motorVertical->setSpeed(0);
-// motorVertical->run(FORWARD);
-// // turn on motor
-// motorVertical->run(RELEASE);
-// Serial.println(SEEKING_SPEED);
-// motorLeft->setSpeed(SEEKING_SPEED);
-// motorLeft->run(FORWARD);
-// motorRight->setSpeed(SEEKING_SPEED);
-
-// if (abs(Outputx) < 125){
-// motorRight->setSpeed(BASE_SPEED - Outputx);
-// motorRight->run(BACKWARD);
-// motorLeft->setSpeed(BASE_SPEED + Outputx); //this need to be higher
-// motorLeft->run(BACKWARD);
-// } else if (Outputx >= 125) { //propeller push in opposite direction, moving to the right
-// motorRight->setSpeed(Outputx);
-// motorRight->run(FORWARD);
-// motorLeft->setSpeed(255); //this need to be higher
-// motorLeft->run(BACKWARD);
-// } else {
-// motorRight->setSpeed(255);
-// motorRight->run(BACKWARD);
-// motorLeft->setSpeed(Outputx); //this need to be higher
-// motorLeft->run(FORWARD);
-// }
-
-// motorVertical->setSpeed(0);
-// motorVertical->run(FORWARD);
-// // turn on motor
-// motorVertical->run(RELEASE);
-// motorLeft->setSpeed(0);
-// motorLeft->run(FORWARD);
-// motorRight->setSpeed(0);
-// motorRight->run(BACKWARD);
-// motorRight->run(BACKWARD);
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.h
deleted file mode 100644
index a1ad9adbad7124479fd8d5168a7b1b24a915521c..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test1_PID/utilities.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef UTI_H
-#define UTI_H
-
-void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B );
-void translateCodetoThreshold(int code);
-
-
-#endif
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.cpp
deleted file mode 100644
index fb27cf808991a0b2083ca36f412ba1aa088527e6..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-#include "Camera.h"
-#include <Wire.h>
-#include <Arduino.h>
-#include <openmvrpc.h>
-
-Camera::Camera(openmv::rpc_i2c_master *intface){
- interface = intface;
-}
-
-void Camera::exe_face_detection(){
- struct { uint16_t x, y, w, h; } face_detection_result;
- if (interface->call_no_args(F("face_detection"), &face_detection_result, sizeof(face_detection_result))) {
- Serial.print(F("Largest Face Detected [x="));
- Serial.print(face_detection_result.x);
- Serial.print(F(", y="));
- Serial.print(face_detection_result.y);
- Serial.print(F(", w="));
- Serial.print(face_detection_result.w);
- Serial.print(F(", h="));
- Serial.print(face_detection_result.h);
- Serial.println(F("]"));
- }
-}
-
-bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
- struct { uint16_t cx, cy, rot; } result;
- if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
- }
- if (result.cx == 0 && result.cy == 0 && result.rot == 0){
- return false;
- } else {
- *x_temp = result.cx;
- *y_temp = result.cy;
- *angle_temp = result.rot;
- return true;
- }
-}
-
-bool Camera::exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max){
- int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
- short buff[128 + 1] = {};
- if (interface->call(F("color_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
- int i = 0;
- while (buff[i] != '\0' && i<100) {
- Serial.print(buff[i]);
- i++;
- }
- Serial.println("");
- }
- if (buff[0] == 0){ //no blob detected
- return false;
- } else {
- return true;
- }
-}
-
-bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y){
- int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
- struct { uint16_t cx, cy; } color_detection_result;
- if (interface->call(F("color_detection_single"), color_thresholds, sizeof(color_thresholds), &color_detection_result, sizeof(color_detection_result))) {
-
- }
- x = color_detection_result.cx;
- y = color_detection_result.cy;
- if (x == 0 && y == 0){
- return false;
- } else {
- return true;
- }
-}
-
-
-bool Camera::exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz){
- int8_t goalid[3] = {goal1, goal2, goal3};
- struct { uint16_t cid, ctx, cty, ctz, crx, cry, crz; } goalfinder_result;
- if(interface->call(F("goalfinder"), goalid, sizeof(goalid), &goalfinder_result, sizeof(goalfinder_result))){
-
- }
- if (goalfinder_result.crx == 0 && goalfinder_result.cry == 0){
- return false;
- } else {
- id = goalfinder_result.cid;
- tx = goalfinder_result.ctx;
- ty = goalfinder_result.cty;
- tz = goalfinder_result.ctz;
- rx = goalfinder_result.crx;
- ry = goalfinder_result.cry;
- rz = goalfinder_result.crz;
- return true;
- }
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.h
deleted file mode 100644
index 5a4536e6d0967e96fd5864e596b8f8f9f0312289..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/Camera.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef CAMERA_H
-#define CAMERA_H
-#include <openmvrpc.h>
-
-class Camera
-{
- private:
- openmv::rpc_i2c_master *interface;
-
- public:
- Camera(openmv::rpc_i2c_master *intface);
- void exe_face_detection(); // Face should be about 2ft away.
- bool exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp);
- bool exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max);
- bool exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y);
- bool exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz); //optional to add tag size as parameter?
-
-};
-#endif
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.cpp
deleted file mode 100644
index eb4ee53b1e31f36bf48e1ea4cf8ecff909fb74c3..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-#include "LedPanel.h"
-
-LedPanel::LedPanel(int pixelnum, int pixelpin){
- led_num = pixelnum;
- //pixels = Adafruit_NeoPixel(pixelnum, pixelpin, NEO_GRB + NEO_KHZ800);
-}
-
-void LedPanel::fullPanelLight(int r, int g, int b){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, r,g,b);
- }
- pixels.show();
-}
-
-void LedPanel::beginPanel(){
- pixels.begin();
-}
-
-void LedPanel::resetPanel(){
- for (int i=0 ; i<led_num ; i++){
- pixels.setPixelColor(i, 0,0,0);
- }
- pixels.show();
-}
-
-void LedPanel::topLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomLeftQuadrant(int r, int g, int b){
- for (int i = 0; i<HORIZONTAL_SIZE/2; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::topRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=0; j<VERTICAL_SIZE/2;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::bottomRightQuadrant(int r, int g, int b){
- for (int i = HORIZONTAL_SIZE/2; i<HORIZONTAL_SIZE; i++){
- for (int j=VERTICAL_SIZE/2; j<VERTICAL_SIZE;j++){
- pixels.setPixelColor(i+j*HORIZONTAL_SIZE, r, g, b);
- }
- }
- pixels.show();
-}
-
-void LedPanel::singleLED(int num, int r, int g, int b) {
- pixels.setPixelColor(num, r, g, b);
- pixels.show();
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.h
deleted file mode 100644
index 41721cc9dd7750951e5345169c2eb604d549f704..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/LedPanel.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef LEDPANEL_H
-#define LEDPANEL_H
-
-#include <Adafruit_NeoPixel.h>
-#define NUMPIXELS 32
-#define PIN_PIXELS 32 // Use pin 32 for NeoPixels
-const int HORIZONTAL_SIZE = 8;
-const int VERTICAL_SIZE = 4;
-
-class LedPanel {
- private:
- int led_num;
- Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN_PIXELS, NEO_GRB + NEO_KHZ800);
-
- public:
- LedPanel(int pixelnum, int pixelpin);
- void fullPanelLight(int r, int b, int g);
- void resetPanel();
- void topLeftQuadrant(int r, int g, int b);
- void bottomLeftQuadrant(int r, int g, int b);
- void topRightQuadrant(int r, int g, int b);
- void bottomRightQuadrant(int r, int g, int b);
- void beginPanel();
- void singleLED(int num, int r, int g, int b);
-
-};
-
-#endif
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/feather_main_test3_PID.ino b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/feather_main_test3_PID.ino
deleted file mode 100644
index 59d2a83f7e3b8126e1cab98a2c0132bef8198549..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/feather_main_test3_PID.ino
+++ /dev/null
@@ -1,402 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_MotorShield.h>
-// #include "Adafruit_VL53L0X.h"
-// #include "ArnholdMesh.h"
-#include "LedPanel.h"
-#include <Arduino.h>
-#include <string>
-#include <vector>
-#include "Camera.h"
-#include <PID_v1.h>
-#include "utilities.h"
-
-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 int SEEKING_SPEED = 70;
-const double RESOLUTION_W = 320.0;
-const double RESOLUTION_H = 240.0;
-const int ENDDEMO_TAG = 0;
-const uint32_t THISNODE_ID = 88789821;
-
-//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
-char newPTCL = '1';
-int pau = 0;
-int displayTracking = 0;
-int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 50; //125;
-
-//The debug LEDs that we will be using. Description at:
-const int DEBUG_STATE = 31;
-const int DEBUG_KP = 30;
-const int DEBUG_KI = 29;
-const int DEBUG_KD = 28;
-const int DEBUG_BASESPEED = 27;
-const int DEBUG_THRESHOLD_MIN = 26;
-const int DEBUG_THRESHOLD_MAX = 25;
-const int DEBUG_VERTICALSPEED = 17;
-const int DEBUG_RSPEED = 16;
-const int DEBUG_LSPEED = 24;
-
-//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_L = AFMS.getMotor(2);
-Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(1);
-Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
-Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
-
-Camera cam(&interface);
-LedPanel panel(NUMPIXELS,PIN_PIXELS);
-// ArnholdMesh thisNode;
-
-//Specify the links and initial tuning parameters
-double Kpx=2, Kix=0.05, Kdx=0.25;
-double Kpy=2, Kiy=0.1, Kdy=0.25;
-PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
-PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
-
-void setup() {
- Serial.begin(BAUD_RATE);
- translateCodetoThreshold(findcolor);
- Wire.begin();
- AFMS.begin(); // create with the default frequency 1.6KHz
- interface.begin(); //communication between ESP and OpenMV
- panel.beginPanel();
-
- Setpointx = 160.0;
- Setpointy = 120.0; //the values that the PID will try to reach
- PID_y.SetOutputLimits(-255, 255); //up positive
- PID_x.SetOutputLimits(-255, 255); //left positive
- PID_x.SetMode(AUTOMATIC);
- PID_y.SetMode(AUTOMATIC);
-}
-
-void loop() {
- //
- panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
-
- while(pau == 1){
- // thisNode.update();
- int zero = 0;
- moveVertical(zero);
- moveHorizontal(zero,zero);
- panel.singleLED(DEBUG_STATE, 10, 10, 0);
- Serial.println("pause");
- }
-
- //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)){
-// int zero = 0;
-// moveVertical(zero);
-// moveHorizontal(zero,zero);
-// panel.singleLED(DEBUG_STATE, 0, 0, 10);
-// while(1){
-// Serial.println("end of demo");
-// }
-// }
-
- //if the demo is still ongoing, check to see if there is a desired-color blob
- int x = 0;
- int y = 0;
- panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
- //if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
- int id = -1;
- int tx = 0;
- int ty = 0;
- int tz = 0;
- int rx = 0;
- int ry = 0;
- int rz = 0;
- int8_t goal[3] = {0,1,2};
- if(cam.exe_goalfinder(goal[0], goal[1], goal[2], id, x, y, tz, rx, ry, rz)){
- if (displayTracking > 0){
- displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
- }
- panel.singleLED(DEBUG_STATE, 0, 10, 0);
- Serial.println("blob detected");
- 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.println(Outputy);
- Serial.println(Outputx);
- Serial.print("Outputx: ");
- Serial.println(Outputx);
- //actuate the vertical motor
- moveVertical(Outputy);
- moveHorizontal(Outputx, BASE_SPEED);
-
- } else { //seeking algorithm
- //panel.resetPanel();
- panel.singleLED(DEBUG_STATE, 10, 10, 10);
- Serial.println("seeking...");
- int zero = 0;
- moveVertical(zero);
- moveHorizontal(SEEKING_SPEED, zero);
- }
-}
-
-//vel value should be between -255 to 255 with positive values moving the blimp
-//upward.
-void moveVertical(int vel){
- if (vel > 0) { //up
- panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0);
- motorVertical_L->setSpeed(abs((int) vel));
- motorVertical_R->setSpeed(abs((int) vel));
- motorVertical_L->run(BACKWARD);
- motorVertical_R->run(BACKWARD);
- } else if(vel < 0) { //down
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
- motorVertical_L->setSpeed(abs((int) Outputy));
- motorVertical_R->setSpeed(abs((int) Outputy));
- motorVertical_L->run(FORWARD);
- motorVertical_R->run(FORWARD);
- } else {
- panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
- motorVertical_L->setSpeed(0);
- motorVertical_R->setSpeed(0);
- }
-}
-
-void moveHorizontal(int vel_hori,int base_speed){
- int lspeed = -1*vel_hori + base_speed;
- int rspeed = vel_hori + base_speed;
-
- if (rspeed > 0){
- motorLeft->run(FORWARD);
- Serial.print("rspeed:");
- Serial.println(rspeed);
- } else {
- motorLeft->run(FORWARD);
- }
-
- if (lspeed > 0){
- motorRight->run(BACKWARD);
- Serial.print("lspeed:");
- Serial.println(lspeed);
- } else {
- motorRight->run(FORWARD);
- }
- displaySpeed(lspeed, rspeed);
- motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed)));
- motorRight->setSpeed(min(MAX_SPEED, abs(lspeed)));
-}
-
-void displaySpeed(int lspeed, int rspeed){
- //Serial.println("display speed");
- if (lspeed < 0){
- panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed));
- } else {
- panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0);
- }
-
- if (rspeed < 0){
- panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed));
- } else {
- panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0);
- }
-}
-
-//When using the camera to detect objects such as colored blobs or april tags. This function is
-//useful when only a single object is the target. The approximate position will be marked with an
-//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be
-//called assuming that you have created the LED panel object, which in this code is named "panel".
-//If the LED panel is called something else, just edit the code here
-void displayTrackedObject(int cx, int cy, int w_res, int h_res){
- int lednum = 0;
- int vertshift = 0;
- panel.resetPanel();
- lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
- vertshift = cy/(h_res/4);
- lednum = lednum + 8*vertshift;
- panel.singleLED(lednum, 10, 10 , 10);
-}
-
-
-//Interpret a message to change what color the camera is detecting
-void setColor(String msg) {
- if (msg.length() < 3){
- int val = msg.toInt();
- translateCodetoThreshold(val);
- Serial.println("code detected");
- } else {
- Serial.println("new threshold detected");
- setColorThreshold(msg, threshold, 6);
- }
- for (int j = 0; j < 6; j++){
- Serial.print(threshold[j]);
- Serial.print(" ");
- }
- Serial.println("");
-}
-
-//in case we don't want to maually set each LAB threshold, we can send over an int to
-//use preset threshold values instead
-void translateCodetoThreshold(int code){
- switch(code){
- case 1: //green old = (30, 100, -68, -13, 30, 127)
- //(30,100,-68,2,6,127) - detect the yellow wall as well
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = -93;
- threshold[3] = -5;
- threshold[4] = 13;
- threshold[5] = 127;
- break;
- case 2: //blue
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = -108;
- threshold[3] = -9;
- threshold[4] = 0;
- threshold[5] = -42;
- break;
- case 5: //red (30, 100, 127, 41, 127, 13)
- threshold[0] = 30;
- threshold[1] = 100;
- threshold[2] = 127;
- threshold[3] = 41;
- threshold[4] = 127;
- threshold[5] = 13;
- }
-}
-
-
-//threshold array must have at least six elements. This function helps
-//translating a message with threshold values to ints. Example msg that would
-//be received by this function are "1 2 3 4 5 6" or "-100 70 9 -9 -50 128".
-//NOTE: - the threshold value should be between -128 to 128
-// - the message should not include the command character used by the mesh
-// - suggested command character: 'C'[olor]
-void setColorThreshold(String msg, int8_t thres[], int arraySize){
- int len = msg.length();
- int temp = 0;
- int startpoint = 0;
- for (int i = 0; i < len; i++){
- if (msg[i] == ' '){
- thres[temp] = msg.substring(startpoint,i).toInt();
- startpoint = i + 1;
- temp++;
- }
- if (temp == 5){
- thres[temp] = msg.substring(startpoint).toInt();
- break;
- }
- }
-}
-
-//This function translate a string of message into the constants for a PID controller. Ignoring the
-//first command character, user can input any one, two, or all three parameter to be changed by identifying
-//the new parameter with the capital letter of that specific parameter, each separated with a space.
-//Some of the valid msg examples are:
-// - "P0.02"
-// - "D1.23"
-// - "P0.14 D9.5"
-// - "P0.1 I0.1 D0.1"
-//
-//NOTE:
-// - the parameter doesn't have to be in order. You can do DI or IP in whichever order as long as it follows a valid double value
-// - while the function works if you passed a negative double, the PID controller will not and will produce error.
-// - suggested command character: 'T'[uning]
-void setPIDConstants(String msg, double &p_constant, double &i_constant, double &d_constant){
- double new_p = Kpx;
- double new_i = Kix;
- double new_d = Kdx;
-
- int len = msg.length();
- int startpoint = 0;
- int endpoint = 0;
- for (int i = 0; i < len; i++){
- if (msg[i] == 'P'){
- startpoint = i + 1;
- for (int j = i + 1; j < len; j++){
- if (msg[j] == ' '){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- new_p = msg.substring(startpoint, endpoint).toDouble();
- }
- }
-
- if (msg[i] == 'I'){
- startpoint = i + 1;
- for (int j = i + 1; j < len; j++){
- if (msg[j] == ' '){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- //i_constant = msg.substring(startpoint, endpoint).toDouble();
- new_i = msg.substring(startpoint, endpoint).toDouble();
- }
- }
-
- if (msg[i] == 'D'){
- startpoint = i + 1;
- for (int j = i + 1; j <= len; j++){
- if (msg[j] == ' ' || msg[j] == '\0'){
- endpoint = j;
- break;
- } else {
- endpoint = len;
- }
- }
- if (endpoint > startpoint){ //check to see if it is a valid value
- //Serial.println(msg.substring(startpoint, endpoint));
- //d_constant = msg.substring(startpoint, endpoint).toDouble();
- new_d = msg.substring(startpoint, endpoint).toDouble();
- }
- }
- }
-
- //DEBUGGING SECTION
- unsigned long t = millis();
- debugPIDConstants(DEBUG_KP, p_constant, new_p);
- debugPIDConstants(DEBUG_KI, i_constant, new_i);
- debugPIDConstants(DEBUG_KD, d_constant, new_d);
-
- p_constant = new_p;
- i_constant = new_i;
- d_constant = new_d;
-
- while(millis() < t+2000); //debugging LED will light up for 2 seconds
- panel.singleLED(DEBUG_KP, 0, 0, 0);
- panel.singleLED(DEBUG_KI, 0, 0, 0);
- panel.singleLED(DEBUG_KD, 0, 0, 0);
-}
-
-void debugPIDConstants(int lednum, double oldval, double newval){
- int r, g, b;
- if (newval > oldval){
- panel.singleLED(lednum, 0, 10, 0);
- } else if (newval < oldval){
- panel.singleLED(lednum, 10, 0, 0);
- } else { //equal
- panel.singleLED(lednum, 10, 10, 10);
- }
-}
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.cpp
deleted file mode 100644
index 7a90e33a9ea7f73c7ad525b0919f9c69b4928fda..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-#include <math.h>
-
-//Source: http://www.easyrgb.com/index.php?X=MATH&H=01#text1
-void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B )
-{
- float var_Y = ( l_s + 16. ) / 116.;
- float var_X = a_s / 500. + var_Y;
- float var_Z = var_Y - b_s / 200.;
-
- if ( pow(var_Y,3) > 0.008856 ) var_Y = pow(var_Y,3);
- else var_Y = ( var_Y - 16. / 116. ) / 7.787;
- if ( pow(var_X,3) > 0.008856 ) var_X = pow(var_X,3);
- else var_X = ( var_X - 16. / 116. ) / 7.787;
- if ( pow(var_Z,3) > 0.008856 ) var_Z = pow(var_Z,3);
- else var_Z = ( var_Z - 16. / 116. ) / 7.787;
-
- float X = 95.047 * var_X ; //ref_X = 95.047 Observer= 2°, Illuminant= D65
- float Y = 100.000 * var_Y ; //ref_Y = 100.000
- float Z = 108.883 * var_Z ; //ref_Z = 108.883
-
-
- var_X = X / 100. ; //X from 0 to 95.047 (Observer = 2°, Illuminant = D65)
- var_Y = Y / 100. ; //Y from 0 to 100.000
- var_Z = Z / 100. ; //Z from 0 to 108.883
-
- float var_R = var_X * 3.2406 + var_Y * -1.5372 + var_Z * -0.4986;
- float var_G = var_X * -0.9689 + var_Y * 1.8758 + var_Z * 0.0415;
- float var_B = var_X * 0.0557 + var_Y * -0.2040 + var_Z * 1.0570;
-
- if ( var_R > 0.0031308 ) var_R = 1.055 * pow(var_R , ( 1 / 2.4 )) - 0.055;
- else var_R = 12.92 * var_R;
- if ( var_G > 0.0031308 ) var_G = 1.055 * pow(var_G , ( 1 / 2.4 ) ) - 0.055;
- else var_G = 12.92 * var_G;
- if ( var_B > 0.0031308 ) var_B = 1.055 * pow( var_B , ( 1 / 2.4 ) ) - 0.055;
- else var_B = 12.92 * var_B;
-
- R = var_R * 255.;
- G = var_G * 255.;
- B = var_B * 255.;
-
-}
-
-
-
-
-
-//old codes
-
-// motorVertical->setSpeed(0);
-// motorVertical->run(FORWARD);
-// // turn on motor
-// motorVertical->run(RELEASE);
-// Serial.println(SEEKING_SPEED);
-// motorLeft->setSpeed(SEEKING_SPEED);
-// motorLeft->run(FORWARD);
-// motorRight->setSpeed(SEEKING_SPEED);
-
-// if (abs(Outputx) < 125){
-// motorRight->setSpeed(BASE_SPEED - Outputx);
-// motorRight->run(BACKWARD);
-// motorLeft->setSpeed(BASE_SPEED + Outputx); //this need to be higher
-// motorLeft->run(BACKWARD);
-// } else if (Outputx >= 125) { //propeller push in opposite direction, moving to the right
-// motorRight->setSpeed(Outputx);
-// motorRight->run(FORWARD);
-// motorLeft->setSpeed(255); //this need to be higher
-// motorLeft->run(BACKWARD);
-// } else {
-// motorRight->setSpeed(255);
-// motorRight->run(BACKWARD);
-// motorLeft->setSpeed(Outputx); //this need to be higher
-// motorLeft->run(FORWARD);
-// }
-
-// motorVertical->setSpeed(0);
-// motorVertical->run(FORWARD);
-// // turn on motor
-// motorVertical->run(RELEASE);
-// motorLeft->setSpeed(0);
-// motorLeft->run(FORWARD);
-// motorRight->setSpeed(0);
-// motorRight->run(BACKWARD);
-// motorRight->run(BACKWARD);
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.h
deleted file mode 100644
index a1ad9adbad7124479fd8d5168a7b1b24a915521c..0000000000000000000000000000000000000000
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test3_PID/utilities.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef UTI_H
-#define UTI_H
-
-void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B );
-void translateCodetoThreshold(int code);
-
-
-#endif