Skip to content
Snippets Groups Projects
Commit 34b7f10a authored by Shahrul Kamil bin Hassan's avatar Shahrul Kamil bin Hassan
Browse files

Delete feather_main_personaldemo.ino

parent dbdbf028
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Code/feather_main_personaldemo.ino deleted 100644 → 0
+ 0
− 514
View file @ dbdbf028
#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"
//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 = 125; //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=1, 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
threshold[0] = 30;
threshold[1] = 100;
threshold[2] = -68;
threshold[3] = -13;
threshold[4] = 30;
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);
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment