diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Camera.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb27cf808991a0b2083ca36f412ba1aa088527e6
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Camera.cpp
@@ -0,0 +1,91 @@
+#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_test_PID_Lidar_Full/Camera.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Camera.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a4536e6d0967e96fd5864e596b8f8f9f0312289
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Camera.h
@@ -0,0 +1,19 @@
+#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_test_PID_Lidar_Full/LedPanel.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/LedPanel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb4ee53b1e31f36bf48e1ea4cf8ecff909fb74c3
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/LedPanel.cpp
@@ -0,0 +1,65 @@
+#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_test_PID_Lidar_Full/LedPanel.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/LedPanel.h
new file mode 100644
index 0000000000000000000000000000000000000000..41721cc9dd7750951e5345169c2eb604d549f704
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/LedPanel.h
@@ -0,0 +1,28 @@
+#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_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino
new file mode 100644
index 0000000000000000000000000000000000000000..3a05f2fb213094bdc33aad8d511202516bb3e49e
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino
@@ -0,0 +1,455 @@
+
+#include <SparkFun_VL53L1X.h>
+#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;
+// ============== lidar part =====================
+SFEVL53L1X distanceSensor;
+int budgetIndex = 4;
+int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
+int LED = LED_BUILTIN;
+//
+
+//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 = 70; //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;
+int lidar_thres = 300; // mm
+
+//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();
+// changed
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
+// changed
+
+Camera cam(&interface);
+LedPanel panel(NUMPIXELS,PIN_PIXELS);
+
+//Specify the links and initial tuning parameters
+double Kpx=2, Kix=0.1, Kdx=0.25;
+double Kpy=1, 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);
+ // ============== lidar part =====================
+ pinMode(LED, OUTPUT);
+ digitalWrite(LED, HIGH);
+ if (distanceSensor.begin() == 0)
+ Serial.println("Sensor online!");
+ distanceSensor.startRanging();
+ distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+ digitalWrite(LED, LOW);
+}
+
+
+// ==============================main loop====================================
+
+int goal_id[3] = {0, 1, 2};
+////goal seeking algorithm
+//if (lidar detect green ball){
+// int id = -1; int tx = 0; int ty = 0; int tz = 0;
+// int rx = 0; int ry = 0; int rz = 0;
+// if(cam.exe_goalfinder(goal_id[0], goal_id[1], goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz)){
+// //apply the same PID control when seeking green ball
+// //if we are at a specific position near the goal (slightly above), move straight forward for 5 second
+// } else {
+// //seeking mechanism to find the goal
+// //move up until we reach the desired altitude
+// //if already at the desired altitude, rotate around
+// }
+//} else { //no ball in basket
+// //do whatever we have been doing before to find a green balloon
+//}
+
+void loop() {
+ panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
+
+ //if the demo is still ongoing, check to see if there is a desired-color blob
+ panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
+
+ //========== lidar part =========
+ int dist = 0;
+ int ball_cap = 0;
+ dist = distanceSensor.getDistance();
+ Serial.println(dist);
+ if ((dist < lidar_thres) && (dist > 0)){ // if we capture the ball
+ ball_cap = 1;
+ Serial.println("find ball");
+ }else{
+ ball_cap = 2;
+ Serial.println("no ball");
+ }
+ // ========== goal finder =========
+ 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};
+
+ int x = 0;
+ int y = 0;
+
+ if (ball_cap == 1){ // if we catch the green ball
+ // we go find the goal
+ if(cam.exe_goalfinder(goal[0],goal[1],goal[2], id, tx, ty, tz, rx, ry, rz)){
+ panel.singleLED(DEBUG_STATE, 0, 10, 0);
+ Inputx = tx/1.00;
+ Inputy = ty/1.00;
+ PID_x.Compute();
+ PID_y.Compute();
+
+ /*
+ Serial.print("x: ");
+ Serial.println(Outputx);
+ Serial.print("y: ");
+ Serial.println(Outputy);
+ */
+ Serial.println("moving toward goal");
+ moveVertical(Outputy);
+ moveHorizontal(Outputx, BASE_SPEED);
+ }else { // seeking
+ seeking();
+ Serial.print("catched the ball and seeking");
+ }
+ }else { // we keep searching for the green ball
+ if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
+// if (displayTracking > 0){
+// displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
+// }
+ Serial.print("catching the ball");
+ panel.singleLED(DEBUG_STATE, 0, 10, 0);
+ Inputx = x/1.00;
+ Inputy = y/1.00;
+ PID_x.Compute();
+ PID_y.Compute();
+
+ //actuate the vertical motor
+ moveVertical(Outputy);
+ moveHorizontal(Outputx, BASE_SPEED);
+
+ } else { //seeking algorithm
+ seeking();
+ Serial.println("seeking the ball ");
+ }
+ }
+
+
+
+}
+// ============================== ^ main loop ^====================================
+
+
+// ============================== other functions ====================================
+
+void seeking(){
+ // Serial.println("seeking...");
+ moveVertical(0);
+ moveHorizontal(SEEKING_SPEED, 0);
+}
+//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);
+ // changed
+ motorVertical_L->setSpeed(abs((int) vel));
+ motorVertical_R->setSpeed(abs((int) vel));
+ motorVertical_L->run(BACKWARD);
+ motorVertical_R->run(FORWARD);
+ // changed
+ } else if(vel < 0) { //down
+ panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel));
+ // changed
+ motorVertical_L->setSpeed(abs((int) vel));
+ motorVertical_R->setSpeed(abs((int) vel));
+ motorVertical_L->run(FORWARD);
+ motorVertical_R->run(BACKWARD);
+
+
+ // changed
+ } else {
+ panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
+ //changed
+ motorVertical_L->setSpeed(0);
+ motorVertical_R->setSpeed(0);
+ //changed
+ }
+}
+
+//LATER WE NEED TO EDIT THIS TO ACCOUNT FOR THE NEW MOTOR
+void moveHorizontal(int vel_hori,int base_speed){
+ int lspeed = -1*vel_hori + base_speed;
+ int rspeed = vel_hori + base_speed;
+
+ if (lspeed > 0){
+ motorLeft->run(BACKWARD); // make it move forward
+ } else {
+ motorLeft->run(FORWARD);
+ }
+ if (rspeed > 0){
+ motorRight-> run(FORWARD);
+ } else {
+ motorRight-> run(BACKWARD); // make it move backward
+ }
+ displaySpeed(lspeed, rspeed);
+ motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )));
+ motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)));
+}
+
+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
+ // LAB: L[min] - L[max], A[min] - A[max], B[min] - B[max]
+ //(30, 100, -49, -22, 31, 127)
+ threshold[0] = 30;
+ threshold[1] = 100;
+ threshold[2] = -49;
+ threshold[3] = -22;
+ threshold[4] = 31;
+ 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_test_PID_Lidar_Full/utilities.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/utilities.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a90e33a9ea7f73c7ad525b0919f9c69b4928fda
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/utilities.cpp
@@ -0,0 +1,83 @@
+#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_test_PID_Lidar_Full/utilities.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/utilities.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1ad9adbad7124479fd8d5168a7b1b24a915521c
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/utilities.h
@@ -0,0 +1,8 @@
+#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