diff --git a/Code/Kamil's Feather Code/feather_main_personaldemo_PID/feather_main_personaldemo_PID.ino b/Code/Kamil's Feather Code/feather_main_personaldemo_PID/feather_main_personaldemo_PID.ino deleted file mode 100644 index 089e84a4650976502bcf6a28e3925d89fd9bf702..0000000000000000000000000000000000000000 --- a/Code/Kamil's Feather Code/feather_main_personaldemo_PID/feather_main_personaldemo_PID.ino +++ /dev/null @@ -1,514 +0,0 @@ -#include <Wire.h> -#include <Adafruit_MotorShield.h> -#include "ArnholdMesh.h" -#include "LedPanel.h" -#include <Arduino.h> -#include <string> -#include <vector> -#include "Camera.h" -#include <PID_v1.h> -#include "utilities.h" - -//definition needed for mesh -#define STATION_SSID "lemur" -#define STATION_PASSWORD "" -#define MQTT_BROKER "" -#define MQTT_TOPIC "" - -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 = 20; //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 = AFMS.getMotor(3); -Adafruit_DCMotor *motorLeft = AFMS.getMotor(2); -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(); - - thisNode.setPanel(&panel); - thisNode.setStationInfo(STATION_SSID, STATION_PASSWORD); - thisNode.setMQTTInfo(MQTT_BROKER, MQTT_TOPIC); - thisNode.amIMQTTBridge(false); - thisNode.init(); - - 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); - -// float r = 0; -// float g = 0; -// float b = 0; -// lab2rgb((float)threshold[0], (float)threshold[2], (float)threshold[4], r, g, b); -// panel.singleLED(DEBUG_THRESHOLD_MIN, (int)r, (int)g, (int)b); -// lab2rgb((float)threshold[1], (float)threshold[3], (float)threshold[5], r, g, b); -// panel.singleLED(DEBUG_THRESHOLD_MAX, (int)r, (int)g, (int)b); -// - // - thisNode.update(); - - 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 -// Serial.print("current threshold: "); -// for (int j = 0; j < 6; j++){ -// Serial.print(threshold[j]); -// Serial.print(" "); -// } -// Serial.println(""); - 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 - } - 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); - - //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->setSpeed(abs((int) vel)); - motorVertical->run(BACKWARD); - } else if(vel < 0) { //down - panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel)); - motorVertical->setSpeed(abs((int) Outputy)); - motorVertical->run(FORWARD); - } else { - panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0); - motorVertical->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(BACKWARD); - } 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); - } -} - - - - -////MESH COMMUNICATION UTILITY FUNCTIONS//// -void interpretDashInstructions(String& msg){ - // Deserialize different dash topics as they are received - String result; - std::list<uint32_t> nodesToSend; - Serial.println(msg); - // If dash topic is Command - if(thisNode.dashDeserialize(msg, "Command", result, nodesToSend)){ - // Interpret what command on the dash means - char numChar = result[0]; - String commandContent = result.substring(1); - if(numChar == 'G'){ - String topo = thisNode.mesh.subConnectionJson(); - thisNode.sendJsonToDash("Debug", topo); - return; - } - else { - thisNode.sendOverAllFreqs(nodesToSend, result); - } - } - // If the dash topic is MasterJoystick - else if(thisNode.dashDeserialize(msg, "MasterJoystick", result, nodesToSend)){ - // Interpret what a dash MasterJoystick message means - String joystickcontrol = "J"; - joystickcontrol += result; - thisNode.sendOverAllFreqs(nodesToSend, result); - return; - } -} - -void vehicleInstruction(char instructionType, String instrData, uint32_t from){ - // Do something with the data receieved over the network - // Based on what the instructionType is, do something different with the instrData - // instructionType CAN NOT BE: - // 'B', 'O', 'g', 'm', 'u', 'R', 'D' - // I am using those within ArnholdMesh for testing purposes - switch(instructionType){ - case 'C': - setColor(instrData); - Serial.print("color: "); - Serial.println(instrData); - break; - case 'T': - setPIDConstants(instrData, Kpx, Kix, Kdx); - Serial.print("new PID constants: "); - Serial.print(Kpx); - Serial.print(" "); - Serial.print(Kix); - Serial.print(" "); - Serial.print(Kdx); - Serial.println(""); - break; - case 'P': //pause the program - pau = instrData.toInt(); - Serial.print("paused: "); - Serial.println(pau); - break; - case 'S': //change the base speed - int new_base_speed = instrData.toInt(); - if (new_base_speed >= 0 && new_base_speed <= 255){ - BASE_SPEED = new_base_speed; - Serial.print("base speed: "); - Serial.println(BASE_SPEED); - } else { - Serial.println("invalid base speed"); - } - break; - } -} - -// Callback function needed for painlessMesh -void receivedCallback(uint32_t from, String &msg) { - Serial.printf("startHere: Received from %u msg=%s\n", from, msg.c_str()); - thisNode.lastMsgReceieved = millis(); - thisNode.colorsL[2] = 5; - thisNode.panel->topLeftQuadrant(thisNode.colorsL[0], thisNode.colorsL[1], thisNode.colorsL[2]); - thisNode.interpretInstruction(from, msg); -} - - -// Callback function needed for MQTT -void mqttCallback(char* topic, uint8_t* payload, unsigned int length) { - char* cleanPayload = (char*)malloc(length+1); - memcpy(cleanPayload, payload, length); - cleanPayload[length] = '\0'; - String msg = String(cleanPayload); - free(cleanPayload); - Serial.println(msg); - interpretDashInstructions(msg); -} - -void newConnectionCallback(uint32_t nodeId) { - Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId); - digitalWrite(LED_BUILTIN, HIGH); - if(thisNode.bridgeId == 0){ - String askpayload = "g"; - thisNode.sendOverAllFreqs(nodeId, askpayload); - } -}