From 460f66c166c7fdf3a5073be3381e9331cdb2356f Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 2 Apr 2023 14:20:44 -0700 Subject: [PATCH 01/16] Pushed development version Pushed an in-progress work that's in the middle of simplifying the Mega's serial telemetry, and allowing it to access all three BLE sensors. --- .DS_Store | Bin 0 -> 6148 bytes Mega-edition/.DS_Store | Bin 0 -> 6148 bytes Mega-edition/Mega_Master/Mega_Master.ino | 258 +++++++++++++++-------- 3 files changed, 165 insertions(+), 93 deletions(-) create mode 100644 .DS_Store create mode 100644 Mega-edition/.DS_Store diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..17073f9ddb4f084df2b6c4a8b42b934d48d45bac GIT binary patch literal 6148 zcmeHK!EVz)5S>j!>!d9N6^RN7$ro;Im8Mk%2c(cDhe&`bxoQu9QoFHI3)ik&(k@`q70_^t*H6dr@32{}8!_{H?_$!zdcX)#4lLEbLpc9rvP|-F?Na13Nx* zlz$k8{_Da+GDv_zpSl<2Ud(!JeDVZ<(iQRy+rbb90ilh_A`U_-}cg>D39&%*j|Z z=Q8!ujODqab&ys-EAT%l!25%V!dO)}N>FYcsN@v@m`ArX`22T(Ii|v@!cl^F0uu=p zm{5hfVu*x}epAI&6^;^2=p@wTL#UaBx}gX;JH|IuyNcdK}kWSj$`TYQM`^K4bMz=fK`Q~ T1knT2e*~lqy3z{#Q3ZYj>k$V2 literal 0 HcmV?d00001 diff --git a/Mega-edition/.DS_Store b/Mega-edition/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..bd1a4f9b45208aff92e190d2337fd64243e18df0 GIT binary patch literal 6148 zcmeHK%}(1u5S|U8Y=Tg8K!T$$y`d`n2(^bwZb%OtkdTbpLt6zqwumLyi(;n%LXcmh zeThB?Pr&2AH@jN|O>;w4g?6ObZ+3QOo&DC?@e+~fjN(nA77DyXDnCfYgvM+SKBma%IIonYr4?%!}fQ~fAY5hD2Ycn^Jjs|D8FtY}=E z+N|s7G)s%J-M$gEh5F))B`KS-xzhX^4AeL%!*Wz~!r^D`9fsu~vi6Zu*}WY`AJb&q zYpw37ybRMk8S3OTP7w0tQ<}$W+)<-EF7!Ns8IZCe8@<;0WU{r}-te|}-b^>V$=fa1 zJKMX{X+y5P+I+Wv{O$Z=ayh;Fp%+8~FR_-F7JtJ}7;{L9i+&d22XpG2knh1T&s2Vb z2##8zBd~pj|45e<_0bzpN;&vH|AX;ghWDVZzw`P9rJrSoP=-CwT=PDz+(m$Ot;1L|HJ(I|KlXra|}2Jo)rV4-VM4P zEXmf^nZn+a literal 0 HcmV?d00001 diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index c004b9b..f9370b2 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -1,11 +1,5 @@ -/* PROGRAM INFORMATION - * - * - * This is the comprehensive arudino sketch for the 2022-2023 Rolling Robots InvenTeam's Walker Project. - * It features, predominantly, the walker's algorithm for navigating rooms to get to its user. - * It also has basic logic for automatic braking. - * - * This sketch is designed to be as liquid as possible with hardware. All inputs and outputs should be customizable and easily integrated. +/* + * CORRECT EDITION 4/2/23 */ @@ -56,6 +50,12 @@ bool decrease_spd = false; int telem_instance_count = 0; + + int rssiF = 0; + int rssiL = 0; + int rssiR = 0; + + // Pins // Analog int LeftHandbrake = 14; // A0 @@ -76,16 +76,16 @@ Left: Serial2 Right: Serial3 BLE Sensors: - No. 1: Serial0 - No. 2: Serial1 - No. 3: Bonus software serial (22, 23) + No. 1: Serial0 (Front) + No. 2: Serial1 (Right) + No. 3: Bonus software serial (22, 23) (Left) LoRa: Software serial (30,31) Only software serial pins have to be specified. All others are set by default by Arduino */ - SoftwareSerial SoftSerialBLE(22, 23); - SoftwareSerial SoftSerialLoRa(30, 31); + SoftwareSerial SoftSerialBLE(52, 53); + SoftwareSerial SoftSerialLoRa(50, 51); // ------- Setup End ------- @@ -112,14 +112,6 @@ } } - void setupMotors(){ - - Serial2.begin(vescbaudrate); - vescML.setSerialPort(&Serial2); - - Serial3.begin(vescbaudrate); - vescMR.setSerialPort(&Serial3); - } void setMotorSpeed(float left, float right){ vescML.setDuty(left / 100); @@ -205,18 +197,14 @@ // ------- Bluetooth Serial Start ------- - - void setupBLE(){ - Serial.begin(9600); - Serial1.begin(9600); - SoftSerialBLE.begin(9600); - } - - const unsigned int maxlength = 7; char telemetry[maxlength]; + char tlmF[maxlength]; + char tlmL[maxlength]; + char tlmR[maxlength]; + //----- String pad3(int input) { // shoves a few zeros on the head of a number @@ -247,80 +235,141 @@ } } +// ---===--- - float getBLERSSI() { + void getRSSIL() { //Comprehensive acquisition of RSSI - String RSSIString; - int i; + // --- DEVELOPMENT OF INFO --- + // Serial command (Individual bytes) + // TLM buffer (Full length command) + // RSSIString (Extracted RSSI value, string form) + // rssi (RSSI value, float form) + + //Serial.listen(); //find serial value - if (telemetry[2] == '-') { - i = 2; - } else { - i = 3; - } - //If the minus sign is there, include it. Else, cut it off (start on next index over) + while (SoftSerialBLE.available() > 0) { //If there are available bytes... + + static unsigned int tlm_pos = 0; //start at index zero + + char inByte = SoftSerialBLE.read(); //get the next byte + if (inByte != '\n' && (tlm_pos < maxlength - 1)) { //if it's before we reach the end... + tlmL[tlm_pos] = inByte; //add to buffer + tlm_pos++; //set index up + } + else { //if we're at the end... + + tlmL[tlm_pos] = '\0'; //close buffer (IS THIS NECCESSARY FOR INTERNAL BUFFER???) - while (i < 6) { - RSSIString = RSSIString + telemetry[i]; // built string out of character array - i++; - } + // --- - return ("%0.2f", (RSSIString.toFloat())); // Format for vesc motors - } + if (tlmL[2] == '+' || tlmL[2] == '-') { //If thing is valid + + String RSSIString; //start up buffer string + int i; if (tlmL[2] == '-') { i = 2; } else { i = 3; } //adjust index to include / exclude sign + while (i < 6) { + RSSIString = RSSIString + tlmL[i]; // built string out of character array + i++; + } + //rssiL = ("%0.2f", ( RSSIString.toFloat() ) ); + rssiL = ( atoi( RSSIString.c_str() ) ); - void parseSerialTelemetry() { + if (debugresponse) { Serial.println(rssiL); } + + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + + } } } - if (debugresponse) { - Serial.print("Command Got!: "); - Serial.println(telemetry); - } +// ---===--- - bool didexecute = true; + void getRSSIR() { //Comprehensive acquisition of RSSI - if (telemetry[0] == '8') { // Is redundant? Or is it useful to have a confirmation that this is correctly-formatted data? + while (Serial1.available() > 0) { //If there are available bytes... + + static unsigned int tlm_pos = 0; //start at index zero + + char inByte = Serial1.read(); //get the next byte + if (inByte != '\n' && (tlm_pos < maxlength - 1)) { //if it's before we reach the end... + tlmR[tlm_pos] = inByte; //add to buffer + tlm_pos++; //set index up + } + else { //if we're at the end... - if (debugresponse) { Serial.println("Purpose: BLE telemetry"); } - - if (telemetry[2] == '+' || telemetry[2] == '-') { - if (debugresponse) { Serial.println("Action: Read RSSI"); } - float rssi = getBLERSSI(); - if (debugresponse) { - Serial.print("Location: "); - Serial.println(telemetry[1]); - Serial.print("BLE RSSI: "); - Serial.println(rssi); - } - } + tlmR[tlm_pos] = '\0'; //close buffer (IS THIS NECCESSARY FOR INTERNAL BUFFER???) - else { - if (debugresponse) { Serial.println("Action: Unrecognized"); } - didexecute = false; - } + // --- - } + if (tlmR[2] == '+' || tlmR[2] == '-') { //If thing is valid + + String RSSIString; //start up buffer string + int i; if (tlmR[2] == '-') { i = 2; } else { i = 3; } //adjust index to include / exclude sign + while (i < 6) { + RSSIString = RSSIString + tlmR[i]; // built string out of character array + i++; + } + + rssiR = ( atoi( RSSIString.c_str() ) ); + + if (debugresponse) { Serial.println(rssiR); } - else{ - if (debugresponse) { Serial.println("Purpose: Unrecognized"); } - didexecute = false; - /* ------------------------------------------------------ * - * Purpose byte, default case: Unknown Command - * ------------------------------------------------------ */ - } - } + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + + } } } +// ---===--- -// ------- Bluetooth Serial End ------- + void getRSSIF() { //Comprehensive acquisition of RSSI + while (Serial.available() > 0) { //If there are available bytes... + + static unsigned int tlm_pos = 0; //start at index zero + + char inByte = Serial.read(); //get the next byte + if (inByte != '\n' && (tlm_pos < maxlength - 1)) { //if it's before we reach the end... + tlmF[tlm_pos] = inByte; //add to buffer + tlm_pos++; //set index up + } + else { //if we're at the end... + + tlmF[tlm_pos] = '\0'; //close buffer (IS THIS NECCESSARY FOR INTERNAL BUFFER???) + // --- -// ------- LoRa Start ------- + if (tlmF[2] == '+' || tlmF[2] == '-') { //If thing is valid + + String RSSIString; //start up buffer string + int i; if (tlmF[2] == '-') { i = 2; } else { i = 3; } //adjust index to include / exclude sign + while (i < 6) { + RSSIString = RSSIString + tlmF[i]; // built string out of character array + i++; + } + + rssiF = ( atoi( RSSIString.c_str() ) ); + + if (debugresponse) { Serial.println(rssiF); } + + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + + } } } + + +// ------- Bluetooth Serial End ------- -void setupLoRa(){ - SoftSerialLoRa.begin(9600); - } +// ------- LoRa Start ------- // ------- LoRa End ------- @@ -421,9 +470,20 @@ void setup() { pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT - setupMotors(); - setupBLE(); - setupLoRa(); // Just starts connection for now + // Motor Setup + Serial2.begin(vescbaudrate); + vescML.setSerialPort(&Serial2); + + Serial3.begin(vescbaudrate); + vescMR.setSerialPort(&Serial3); + + // BLE Setup + Serial.begin(9600); + Serial1.begin(9600); + SoftSerialBLE.begin(9600); + + // LoRa Setup + SoftSerialLoRa.begin(9600); Serial.println("Mega Master Start!"); @@ -434,9 +494,9 @@ void setup() { void loop() { // GET SENSOR DATA - loopUltrasonic(); - loopInfrared(); - loopPressure(); + // loopUltrasonic(); + // loopInfrared(); + // loopPressure(); delay(100); // @@ -445,7 +505,10 @@ void loop() { //=====---===== CONTROLLER =====---===== - FacilityControl(); + //FacilityControl(); + + getRSSIL(); 1` + Serial.println(rssiL); //=====---===== CONTROLLER =====---===== @@ -454,11 +517,7 @@ void loop() { // ACQUIRE TELEMETRY delay(300); - - - telem_instance_count = telem_instance_count + 1; - - if(telem_instance_count < 6){ + /* SoftSerialBLE.listen(); while (SoftSerialBLE.available() > 0) { static unsigned int tlm_pos = 0; @@ -474,8 +533,21 @@ void loop() { tlm_pos = 0; // Await next command } } + +} +*/ + + +/* + + telem_instance_count = telem_instance_count + 1; + + if(telem_instance_count < 6){ + } else { telem_instance_count = 0; } + + */ } From 88df1290d765aef186a9a8c4fb12349edcf5629b Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 2 Apr 2023 15:07:53 -0700 Subject: [PATCH 02/16] Navigation Framework added Have created the space at the bottom of the code where the navigation system will sit. - Added very simple flow from user to nav and back --- Mega-edition/Mega_Master/Mega_Master.ino | 240 ++++++++++++----------- 1 file changed, 130 insertions(+), 110 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index f9370b2..730989c 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -17,6 +17,14 @@ Key: '-' indicates notes, '*' indicates issues that should be looked at --- + + Date: 4/2/23 + + Alex. We're doing this thing. + - Expand the RSSI function to work with all three serial ports. + - Isolate redundant telemetry functions. Comment out until it's verified the code works: then remove. + - Incorporate actuators? into hardware scheme. + - Start on Leo's nav algorithm. Date: 3/19/23 @@ -45,6 +53,13 @@ int vescbaudrate = 9600; // Variables + + int usermode = 1; + // 1 is standard use mode (automatic brake, otherwise a dead walker) (default mode) + // 2 is room nav mode. Starts when beacon ping is recieved, ends when RSSI indicates walker is within ~3 feet of user. + // 3 is debug mode, must be activated manually in-code or through some other method + + int spd_count = 0; bool increase_spd = true; bool decrease_spd = false; @@ -58,17 +73,19 @@ // Pins // Analog - int LeftHandbrake = 14; // A0 - int RightHandbrake = 15; // A1 - int LeftInfared = 16; // A2 - int RightInfared = 17; // A3 + const int LeftHandbrake = 14; // A0 + const int RightHandbrake = 15; // A1 + const int LeftInfared = 16; // A2 + const int RightInfared = 17; // A3 //int ANALOG4 = 18; // A4 //int ANALOG5 = 19; // A5 // Digital and Serial - int UltrasonicTrig = 27; - int UltrasonicEcho = 28; + const int UltrasonicTrig = 27; + const int UltrasonicEcho = 28; + const int FOR_RELAY_PIN = 3; + const int REV_RELAY_PIN = 4; /* All parenthesis in rx -> tx order - Serial (0, 1) - Serial1 (19, 18) - Serial2 (17, 16) - Serial3 (15, 14) @@ -122,6 +139,22 @@ +// ------- Actuator Start ------- + +void extendActuators(){ + digitalWrite(FOR_RELAY_PIN, HIGH); + digitalWrite(REV_RELAY_PIN, LOW); +} + +void retractActuators(){ + digitalWrite(FOR_RELAY_PIN, LOW); + digitalWrite(REV_RELAY_PIN, HIGH); +} + +// ------- Actuator End ------- + + + // ------- Sensor Start ------- // Ultrasonic variableas @@ -235,6 +268,15 @@ } } +// ---===--- + +/* + RSSI Functions: + - Simply run em' once in the update cycle (ideally) + - They output to global variables 'rssiL', 'rssiR', and 'rssiF' + - use variables at your discretion +*/ + // ---===--- void getRSSIL() { //Comprehensive acquisition of RSSI @@ -258,9 +300,9 @@ } else { //if we're at the end... - tlmL[tlm_pos] = '\0'; //close buffer (IS THIS NECCESSARY FOR INTERNAL BUFFER???) + tlmL[tlm_pos] = '\0'; //close buffer (IS THIS NECCESSARY FOR INTERNAL BUFFER???) (a.k.a with known length) - // --- + // --- if (tlmL[2] == '+' || tlmL[2] == '-') { //If thing is valid @@ -274,8 +316,11 @@ rssiL = ( atoi( RSSIString.c_str() ) ); - if (debugresponse) { Serial.println(rssiL); } - + if (debugresponse) { + Serial.print("GetRSSIL - Value: ") + Serial.println(rssiL); + } + } else {} //invalid // --- @@ -286,7 +331,7 @@ // ---===--- - void getRSSIR() { //Comprehensive acquisition of RSSI + void getRSSIR() { while (Serial1.available() > 0) { //If there are available bytes... @@ -314,8 +359,11 @@ rssiR = ( atoi( RSSIString.c_str() ) ); - if (debugresponse) { Serial.println(rssiR); } - + if (debugresponse) { + Serial.print("GetRSSIR - Value: ") + Serial.println(rssiR); + } + } else {} //invalid // --- @@ -326,7 +374,7 @@ // ---===--- - void getRSSIF() { //Comprehensive acquisition of RSSI + void getRSSIF() { while (Serial.available() > 0) { //If there are available bytes... @@ -354,7 +402,10 @@ rssiF = ( atoi( RSSIString.c_str() ) ); - if (debugresponse) { Serial.println(rssiF); } + if (debugresponse) { + Serial.print("GetRSSIF - Value: ") + Serial.println(rssiF); + } } else {} //invalid @@ -375,57 +426,18 @@ // ------- Control Start ------- - int threshUltrasonic = 60; // (reading of about 5 inches) - int threshPressure = 500; // (From about 10, nothing, to about 1000, full grip) - int threshRSSI = -40; // (From -90, furthest, to -30, closest) VERIFY - int threshIR = 40; // (6 inches?) - - // Demonstration Contrller - - void DemoControl(){ - - const int maxspeed = 5; - - static int L = 0; // Establish speed variables - static int R = 0; - - int targetL = maxspeed; // Start by trying to run motors at full - int targetR = maxspeed; - - // Target Shifting - if ( distanceUltrasonic < threshUltrasonic || pressureReadingLeft < threshPressure || pressureReadingRight < threshPressure ) { // Anything to stop both motors - targetL = 0; - targetR = 0; - } - - if ( distanceIRLeft < threshIR ) { - targetL = 0; - } - - if ( distanceIRRight < threshIR ) { - targetR = 0; - } - // - - L = L + between( (targetL - L), -2, 1 ); // Tries to go to target speed, limited in set increments. Works with decimals and not-nice fractions. - R = R + between( (targetR - R), -2, 1 ); // Discrepancy in absolute value of down/up increments is the relative rate of deccel / accel. Here, deccelerates twice as fast. - - setMotorSpeed(L, R); - - if (debugresponse) { - - Serial.print ("Left target speed is "); - Serial.print (targetL); - Serial.print (" , left motor is running at "); - Serial.println (L); - Serial.println (""); - - } - } + void UpdateData(){ + loopUltrasonic(); + loopInfared(); + loopPressure(); + getRSSIF(); + getRSSIL(); + getRSSIR(); + } - + int threshUltrasonic = 60; // (reading of about 5 inches) - void FacilityControl(){ + void Debugger(){ const int maxspeed = 6; @@ -454,13 +466,19 @@ } } - // --- - // ------- Control End ------- // ------- Nav Start ------- + + + + + + + + // ------- Nav End ------- @@ -470,6 +488,9 @@ void setup() { pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT + pinMode(FOR_RELAY_PIN, OUTPUT); + pinMode(REV_RELAY_PIN, OUTPUT); + // Motor Setup Serial2.begin(vescbaudrate); vescML.setSerialPort(&Serial2); @@ -490,64 +511,63 @@ void setup() { } - +// --- === --- void loop() { +// --- === --- - // GET SENSOR DATA - // loopUltrasonic(); - // loopInfrared(); - // loopPressure(); - delay(100); - // +switch( usermode ): + case 3: // Debug mode + Debugger(); + delay(500); + break; - //=====---===== CONTROLLER =====---===== - //FacilityControl(); - getRSSIL(); 1` - Serial.println(rssiL); - - //=====---===== CONTROLLER =====---===== - + default: // User control mode (1) + bool pingedbybeacon = false; - // ACQUIRE TELEMETRY - delay(300); + /* - /* - SoftSerialBLE.listen(); - while (SoftSerialBLE.available() > 0) { - static unsigned int tlm_pos = 0; - char inByte = SoftSerialBLE.read(); - if (inByte != '\n' && (tlm_pos < maxlength - 1)) { - telemetry[tlm_pos] = inByte; - tlm_pos++; - } - else { - telemetry[tlm_pos] = '\0'; - parseSerialTelemetry(); - Serial.println(""); - tlm_pos = 0; // Await next command - } + Do user-related stuff + - Activate autobrake + - Listen out for watch signal + + */ + + if (pingedbybeacon){ + usermode = 2; } - -} -*/ + break; -/* - telem_instance_count = telem_instance_count + 1; - - if(telem_instance_count < 6){ - - } else { - telem_instance_count = 0; - } - */ - + case 2: // Room navigation mode + + bool withinrange = false; + + while (withinrange == false){ + + // Do whole navigation algorithm here + + /* eventually meeting the RSSI condition: + withinrange = true; + */ + + delay(500); + + } + + usermode = 1; + + break; + + + +// --- === --- } +// --- === --- \ No newline at end of file From ff3eeb76bb73a928e436485728b227fbfe3e1608 Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 2 Apr 2023 15:12:24 -0700 Subject: [PATCH 03/16] Literally 1 line change Added the data updater to the navigation loop --- Mega-edition/Mega_Master/Mega_Master.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 730989c..e74f335 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -551,7 +551,7 @@ switch( usermode ): bool withinrange = false; while (withinrange == false){ - + UpdateData(); // Do whole navigation algorithm here /* eventually meeting the RSSI condition: From e525182fcd31915a35d43a4aaa43471cc020a41a Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 2 Apr 2023 15:30:28 -0700 Subject: [PATCH 04/16] Arduino error fixes A few missed semicolons --- Mega-edition/Mega_Master/Mega_Master.ino | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index e74f335..8b10ef8 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -190,7 +190,7 @@ void retractActuators(){ int distanceIRLeft; int distanceIRRight; - void loopInfrared() { + void loopIR() { float voltsL = analogRead(LeftInfared)*0.0048828125; // value from sensor * (5/1024) distanceIRLeft = 13*pow(voltsL, -1); // worked out from datasheet graph @@ -317,7 +317,7 @@ void retractActuators(){ rssiL = ( atoi( RSSIString.c_str() ) ); if (debugresponse) { - Serial.print("GetRSSIL - Value: ") + Serial.print("GetRSSIL - Value: "); Serial.println(rssiL); } @@ -360,7 +360,7 @@ void retractActuators(){ rssiR = ( atoi( RSSIString.c_str() ) ); if (debugresponse) { - Serial.print("GetRSSIR - Value: ") + Serial.print("GetRSSIR - Value: "); Serial.println(rssiR); } @@ -403,7 +403,7 @@ void retractActuators(){ rssiF = ( atoi( RSSIString.c_str() ) ); if (debugresponse) { - Serial.print("GetRSSIF - Value: ") + Serial.print("GetRSSIF - Value: "); Serial.println(rssiF); } @@ -428,7 +428,7 @@ void retractActuators(){ void UpdateData(){ loopUltrasonic(); - loopInfared(); + loopIR(); loopPressure(); getRSSIF(); getRSSIL(); @@ -517,7 +517,7 @@ void loop() { -switch( usermode ): +switch( usermode ){ case 3: // Debug mode Debugger(); @@ -568,6 +568,8 @@ switch( usermode ): +} + // --- === --- } -// --- === --- \ No newline at end of file +// --- === --- From 22a376b58851fc711e10d58fabfae1e8c158ffcf Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Mon, 3 Apr 2023 06:17:47 -0700 Subject: [PATCH 05/16] Improved Debug Response Each instance of 'if debugresponse, print' has the function print its own name first: making it (hopefully) clearer which parts of text come from where --- Mega-edition/Mega_Master/Mega_Master.ino | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 8b10ef8..d3ed0ea 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -178,7 +178,7 @@ void retractActuators(){ distanceUltrasonic = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back) if ( debugresponse ) { - Serial.print ( "Ultrasonic distance: " ); + Serial.print ( "loopUltrasonic: Ultrasonic distance: " ); Serial.println ( distanceUltrasonic ); } @@ -198,7 +198,7 @@ void retractActuators(){ distanceIRRight = 13*pow(voltsR, -1); // worked out from datasheet graph if ( debugresponse ) { - Serial.print ( "Infared distances: Left: " ); + Serial.print ( "loopIR - Infared distances: Left: " ); Serial.print ( distanceIRLeft ); Serial.print ( " Right: " ); Serial.println ( distanceIRRight ); @@ -216,7 +216,7 @@ void retractActuators(){ pressureReadingRight = analogRead(RightHandbrake); if ( debugresponse ) { - Serial.print ( "Pressure reading: Left: " ); + Serial.print ( "loopPressure - Pressure reading: Left: " ); Serial.print ( pressureReadingLeft ); Serial.print ( " Right: " ); Serial.println ( pressureReadingRight ); @@ -457,7 +457,7 @@ void retractActuators(){ if (debugresponse) { - Serial.print ("Target speed is "); + Serial.print ("Debug Controller - Target speed is "); Serial.print (target); Serial.print (" , Motor duty is set to "); Serial.println (Speed); @@ -554,7 +554,7 @@ switch( usermode ){ UpdateData(); // Do whole navigation algorithm here - /* eventually meeting the RSSI condition: + /* eventually meeting the RSSI condition:33 withinrange = true; */ From 8dfdeeba93e065fc6f2388f64068e0601885ccad Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Thu, 13 Apr 2023 20:58:38 -0700 Subject: [PATCH 06/16] Loop Reworking Reworked the main functional loop: - Major functions (user control, nav) placed in their own small while loops, to be more economical - Simple switching between functions - Failsafe to avoid getting caught by false breaking from usercontrol loop --- Mega-edition/Mega_Master/Mega_Master.ino | 75 +++++++++++------------- 1 file changed, 35 insertions(+), 40 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index d3ed0ea..72eb632 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -54,10 +54,6 @@ // Variables - int usermode = 1; - // 1 is standard use mode (automatic brake, otherwise a dead walker) (default mode) - // 2 is room nav mode. Starts when beacon ping is recieved, ends when RSSI indicates walker is within ~3 feet of user. - // 3 is debug mode, must be activated manually in-code or through some other method int spd_count = 0; @@ -511,64 +507,63 @@ void setup() { } -// --- === --- -void loop() { -// --- === --- + bool break; + int reason; // it switches to several other loops. + // 1 is room nav + // 2 is debug looping + // Anything else does nothing, and maybe logs an error message for now. + // After the reason is taken care of, break is reset, and the main business comes -switch( usermode ){ - case 3: // Debug mode - Debugger(); - delay(500); - break; +// --- === --- +void loop() { +// --- === --- +// UpdateData(); + break == false; + reason = 0; + + do { // Run the user navigation + // If condition met: + // break = true; + // set reason to specific number + delay(500); - default: // User control mode (1) + } while (break == false); - bool pingedbybeacon = false; + switch(reason): - /* + case 1: // Navigation + bool targettreached = false; + do { - Do user-related stuff - - Activate autobrake - - Listen out for watch signal - - */ + // NAVVVV + // something that sets targetreached to true + delay(500); - if (pingedbybeacon){ - usermode = 2; - } + } while ( targetreached == false ); break; - case 2: // Room navigation mode - - bool withinrange = false; - - while (withinrange == false){ - UpdateData(); - // Do whole navigation algorithm here - - /* eventually meeting the RSSI condition:33 - withinrange = true; - */ - + case 2: // Debugging + while(1){ + Debugger(); delay(500); - + // stop whenever?? } - - usermode = 1; - break; -} + default: // What?? + // Log error message + delay(500); + break; // --- === --- } From 8a56107ba0d54408db6a411cd7cd4942267f1d77 Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 16 Apr 2023 19:00:26 -0700 Subject: [PATCH 07/16] Motor Functionality Added Arduino can send commands effectively, and can receive all telemetry properly. --- Mega-edition/Mega_Master/Mega_Master.ino | 108 ++++++++++++++++++----- 1 file changed, 88 insertions(+), 20 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 72eb632..76831b2 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -14,9 +14,9 @@ /* --- Latest Update Log --- - Key: '-' indicates notes, '*' indicates issues that should be looked at - - --- + Date: 4/16/23 + + Alex. Trying to add Vesc data. Date: 4/2/23 @@ -54,8 +54,6 @@ // Variables - - int spd_count = 0; bool increase_spd = true; bool decrease_spd = false; @@ -109,6 +107,9 @@ VescUart vescML; VescUart vescMR; + int oldSpeedL = 0; + int oldSpeedR = 0; + float getMotorRPM(VescUart vesc) { if (vesc.getVescValues()) { float motor_rpm = vesc.data.rpm; @@ -128,9 +129,38 @@ void setMotorSpeed(float left, float right){ vescML.setDuty(left / 100); - vescMR.setDuty(right / 100); + vescMR.setDuty(right / 100); } + void setMotorSpeedBetter(float left, float right){ + if (left != oldSpeedL) { + + vescML.setDuty(left / 100); + oldSpeedL = left; + + if ( debugresponse ) { + Serial.print ( "setMotorSpeed: Left speed changed: " ); + Serial.println ( left ); + } + + } else { + Serial.println ( "setMotorSpeed: Left speed unchanged" ); + } + + + if (right != oldSpeedR) { + vescMR.setDuty(right / 100); + oldSpeedR = right; + + if ( debugresponse ) { + Serial.print ( "setMotorSpeed: Right speed changed: " ); + Serial.println ( right ); + } + + } else { + Serial.println ( "setMotorSpeed: Right speed unchanged" ); + }} + // ------- Motor End ------- @@ -508,7 +538,7 @@ void setup() { - bool break; + bool breakout; int reason; // it switches to several other loops. // 1 is room nav // 2 is debug looping @@ -522,29 +552,67 @@ void loop() { // --- === --- // UpdateData(); - break == false; + breakout == false; reason = 0; do { // Run the user navigation - // If condition met: - // break = true; - // set reason to specific number + Serial.println("I am stuck in the loop"); + loopUltrasonic(); + loopIR(); + loopPressure(); + getRSSIF(); + getRSSIL(); + getRSSIR(); + if (vescML.getVescValues() ){ + Serial.print("Left RPM: "); + Serial.print(vescML.data.rpm); + Serial.print(" | Tachometer: "); + Serial.println(vescML.data.tachometerAbs); + } + else { Serial.println("Left Data Failed!"); } + + if (vescMR.getVescValues() ){ + Serial.print("Right RPM: "); + Serial.print(vescMR.data.rpm); + Serial.print(" | Tachometer: "); + Serial.println(vescMR.data.tachometerAbs); + } + else { Serial.println("Right Data Failed!"); } + + //setMotorSpeed(5,5); + delay(500); - } while (break == false); + } while (breakout == false); - switch(reason): + switch(reason){ case 1: // Navigation - bool targettreached = false; - do { - // NAVVVV - // something that sets targetreached to true - delay(500); - } while ( targetreached == false ); +/* + * Variable Key: + * + * Ultrasonic distance at the front: distanceUltrasonic + * Infared distance to left: distanceIRLeft + * Infared distance to right: distanceIRRight + * Left pressure sensor: pressureReadingLeft + * Right pressure sensor: pressureReadingRight + * RSSI for front: rssiF + * RSSI for left: rssiL + * RSSI for right: rssiR + * + * Format for vesc motor telemetry: [motor channel].data.[type of information] + * Motor channels (2): vescML, vescMR + * Types of information (4): rpm, inpVoltage, ampHours, tachometerAbs + * + */ + + + + bool targetreached = false; + while(targetreached == false){ delay(500); } break; @@ -566,5 +634,5 @@ void loop() { break; // --- === --- -} +} } // --- === --- From 19077e813322eb5d136632e034c5b33b5dd595c2 Mon Sep 17 00:00:00 2001 From: ColColA <71855826+ColColA@users.noreply.github.com> Date: Sun, 23 Apr 2023 17:27:14 -0700 Subject: [PATCH 08/16] made it run constant added a call of the setMotorSpeed function @ line 580 to make the walker constantly move forward. Also did some formatting --- .DS_Store | Bin 6148 -> 6148 bytes Mega-edition/.DS_Store | Bin 6148 -> 6148 bytes Mega-edition/Mega_Master/Mega_Master.ino | 9 +++------ 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/.DS_Store b/.DS_Store index 17073f9ddb4f084df2b6c4a8b42b934d48d45bac..bbe858b3b45cd4d5c729eeaae5810ccdf90ef28a 100644 GIT binary patch delta 34 qcmZoMXfc@J&&a9%MSq8t_vst delta 36 scmZoMXfc@J&&awlU^gQp>tsV_sm*Fk9*mnOGMBMTY*60J&heKY0LE7e*#H0l diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 76831b2..2a2a258 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -79,7 +79,7 @@ const int UltrasonicTrig = 27; const int UltrasonicEcho = 28; const int FOR_RELAY_PIN = 3; - const int REV_RELAY_PIN = 4; + const int REV_RELAY_PIN = 4; /* All parenthesis in rx -> tx order - Serial (0, 1) - Serial1 (19, 18) - Serial2 (17, 16) - Serial3 (15, 14) @@ -536,8 +536,6 @@ void setup() { } - - bool breakout; int reason; // it switches to several other loops. // 1 is room nav @@ -546,7 +544,6 @@ void setup() { // After the reason is taken care of, break is reset, and the main business comes - // --- === --- void loop() { // --- === --- @@ -579,9 +576,9 @@ void loop() { Serial.println(vescMR.data.tachometerAbs); } else { Serial.println("Right Data Failed!"); } + + setMotorSpeed(-4, -4); - //setMotorSpeed(5,5); - delay(500); } while (breakout == false); From 31caa8b91163acc43e833fdac72b5b29619cc950 Mon Sep 17 00:00:00 2001 From: ColColA <71855826+ColColA@users.noreply.github.com> Date: Sun, 30 Apr 2023 18:43:49 -0700 Subject: [PATCH 09/16] Stuff works now IMU code added, added a distance stop in navigation area to test stuff, got the IR sensors to work. --- Mega-edition/Mega_Master/Mega_Master.ino | 538 ++++++++++++++++------- 1 file changed, 389 insertions(+), 149 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 2a2a258..d646a10 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -1,5 +1,5 @@ /* - * CORRECT EDITION 4/2/23 + * CORRECT EDITION 4/23/23 */ @@ -14,6 +14,10 @@ /* --- Latest Update Log --- + Date: 4/23/23 + + Alex. Adding the debug button. It puts it in debug / disable ! + Date: 4/16/23 Alex. Trying to add Vesc data. @@ -47,6 +51,7 @@ #include #include #include + #include // Constants bool debugresponse = true; @@ -64,22 +69,27 @@ int rssiL = 0; int rssiR = 0; + + bool disable() { + return (!digitalRead(22) ); + } + // Pins // Analog - const int LeftHandbrake = 14; // A0 - const int RightHandbrake = 15; // A1 - const int LeftInfared = 16; // A2 - const int RightInfared = 17; // A3 - //int ANALOG4 = 18; // A4 - //int ANALOG5 = 19; // A5 + const int LeftHandbrake = A0; + const int RightHandbrake = A1; + const int LeftInfared = A5; + const int RightInfared = A4; // Digital and Serial const int UltrasonicTrig = 27; const int UltrasonicEcho = 28; const int FOR_RELAY_PIN = 3; - const int REV_RELAY_PIN = 4; + const int REV_RELAY_PIN = 4; + + const int DebugSwitchPin = 22; /* All parenthesis in rx -> tx order - Serial (0, 1) - Serial1 (19, 18) - Serial2 (17, 16) - Serial3 (15, 14) @@ -100,8 +110,6 @@ // ------- Setup End ------- - - // ------- Motor Start ------- VescUart vescML; @@ -161,26 +169,44 @@ Serial.println ( "setMotorSpeed: Right speed unchanged" ); }} -// ------- Motor End ------- - + void motordebug(){ + if(debugresponse){ + + if (vescML.getVescValues() ){ + Serial.print("Left RPM: "); + Serial.print(vescML.data.rpm); + Serial.print(" | Tachometer: "); + Serial.println(vescML.data.tachometerAbs); + } + else { Serial.println("Left Data Failed!"); } + + if (vescMR.getVescValues() ){ + Serial.print("Right RPM: "); + Serial.print(vescMR.data.rpm); + Serial.print(" | Tachometer: "); + Serial.println(vescMR.data.tachometerAbs); + } + else { Serial.println("Right Data Failed!"); } + + } else{} + } +// ------- Motor End ------- // ------- Actuator Start ------- -void extendActuators(){ - digitalWrite(FOR_RELAY_PIN, HIGH); - digitalWrite(REV_RELAY_PIN, LOW); -} + void extendActuators(){ + digitalWrite(FOR_RELAY_PIN, HIGH); + digitalWrite(REV_RELAY_PIN, LOW); + } -void retractActuators(){ - digitalWrite(FOR_RELAY_PIN, LOW); - digitalWrite(REV_RELAY_PIN, HIGH); -} + void retractActuators(){ + digitalWrite(FOR_RELAY_PIN, LOW); + digitalWrite(REV_RELAY_PIN, HIGH); + } // ------- Actuator End ------- - - // ------- Sensor Start ------- // Ultrasonic variableas @@ -225,9 +251,9 @@ void retractActuators(){ if ( debugresponse ) { Serial.print ( "loopIR - Infared distances: Left: " ); - Serial.print ( distanceIRLeft ); + Serial.print ( voltsL ); Serial.print ( " Right: " ); - Serial.println ( distanceIRRight ); + Serial.println ( voltsR ); } } @@ -252,8 +278,6 @@ void retractActuators(){ // ------- Sensor End ------- - - // ------- Bluetooth Serial Start ------- const unsigned int maxlength = 7; @@ -293,19 +317,16 @@ void retractActuators(){ Serial.println(indivbyte); } } + /* + RSSI Functions: + - Simply run em' once in the update cycle (ideally) + - They output to global variables 'rssiL', 'rssiR', and 'rssiF' + - use variables at your discretion + */ -// ---===--- + // getRSSI Left -/* - RSSI Functions: - - Simply run em' once in the update cycle (ideally) - - They output to global variables 'rssiL', 'rssiR', and 'rssiF' - - use variables at your discretion -*/ - -// ---===--- - - void getRSSIL() { //Comprehensive acquisition of RSSI + void getRSSIL() { //Comprehensive acquisition of RSSI // --- DEVELOPMENT OF INFO --- // Serial command (Individual bytes) @@ -313,7 +334,7 @@ void retractActuators(){ // RSSIString (Extracted RSSI value, string form) // rssi (RSSI value, float form) - //Serial.listen(); //find serial value + SoftSerialBLE.listen(); //find serial value while (SoftSerialBLE.available() > 0) { //If there are available bytes... @@ -353,9 +374,11 @@ void retractActuators(){ tlm_pos = 0; // Await next command - } } } + } + } + } -// ---===--- + // getRSSI Right void getRSSIR() { @@ -396,9 +419,11 @@ void retractActuators(){ tlm_pos = 0; // Await next command - } } } + } + } + } -// ---===--- + // getRSSI Forward void getRSSIF() { @@ -439,26 +464,112 @@ void retractActuators(){ tlm_pos = 0; // Await next command - } } } + } + } + } - // ------- Bluetooth Serial End ------- - - // ------- LoRa Start ------- // ------- LoRa End ------- +// ------- IMU Start ------- + // Basic demo for accelerometer & gyro readings from Adafruit + // LSM6DSO32 sensor + + #include + + // For SPI mode, we need a CS pin + #define LSM_CS 12 + // For software-SPI mode we need SCK/MOSI/MISO pins + #define LSM_SCK 13 + #define LSM_MISO 12 + #define LSM_MOSI 11 + + float distance_x; + + Adafruit_LSM6DSO32 dso32; + + void IMUloop() { + + // /* Get a new normalized sensor event */ + sensors_event_t accel; + sensors_event_t gyro; + sensors_event_t temp; + dso32.getEvent(&accel, &gyro, &temp); + + float accel_x_tare; + accel_x_tare = 0.5; + float accel_x_tared; + + // Serial.print("\t\tTemperature "); + // Serial.print(temp.temperature); + // Serial.println(" deg C"); + + /* Display the results (acceleration is measured in m/s^2) */ + Serial.print("\t\tAccel X: "); + Serial.print(accel.acceleration.x); + Serial.print("\t\tAccel X Tared: "); + if (abs(accel.acceleration.x) > 0.2){ + accel_x_tared = accel.acceleration.x + accel_x_tare; + } else { + accel_x_tared = accel.acceleration.x; + } + Serial.print(accel_x_tared); + + + if (abs(accel_x_tared) >= 0.2){ + distance_x = distance_x + (0.5 * (accel_x_tared) * pow(0.1, 2)); + } + // Serial.print(" \tY: "); + // Serial.print(accel.acceleration.y); + // Serial.print(" \tZ: "); + // Serial.print(accel.acceleration.z); + Serial.println(" m/s^2 "); + + Serial.print(distance_x); + Serial.println(" m "); + + // /* Display the results (rotation is measured in rad/s) */ + // Serial.print("\t\tGyro X: "); + // Serial.print(gyro.gyro.x); + // Serial.print(" \tY: "); + // Serial.print(gyro.gyro.y); + // Serial.print(" \tZ: "); + // Serial.print(gyro.gyro.z); + // Serial.println(" radians/s "); + // Serial.println(); + + delay(100); + + // // serial plotter friendly format + + // Serial.print(temp.temperature); + // Serial.print(","); + + // Serial.print(accel.acceleration.x); + // Serial.print(","); Serial.print(accel.acceleration.y); + // Serial.print(","); Serial.print(accel.acceleration.z); + // Serial.print(","); + + // Serial.print(gyro.gyro.x); + // Serial.print(","); Serial.print(gyro.gyro.y); + // Serial.print(","); Serial.print(gyro.gyro.z); + // Serial.println(); + // delayMicroseconds(10000); + } +// ------- IMU End ------- // ------- Control Start ------- void UpdateData(){ - loopUltrasonic(); - loopIR(); - loopPressure(); - getRSSIF(); - getRSSIL(); - getRSSIR(); + // loopUltrasonic(); + // loopIR(); + // loopPressure(); + // getRSSIF(); + // getRSSIL(); + // getRSSIR(); + // motordebug(); } int threshUltrasonic = 60; // (reading of about 5 inches) @@ -494,142 +605,271 @@ void retractActuators(){ // ------- Control End ------- +// ------- Nav Start ------- + void collisionDetect() { + if (distanceUltrasonic > 30) { + setMotorSpeed(-5,-5); + } else { + setMotorSpeed(0,0); + } + } -// ------- Nav Start ------- +// ------- Nav End ------- +// ------- Setup Function Start ------- + void setup() { + pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT + pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT + pinMode(FOR_RELAY_PIN, OUTPUT); + pinMode(REV_RELAY_PIN, OUTPUT); + pinMode(LeftInfared, INPUT); + pinMode(RightInfared, INPUT); + pinMode(SDA, INPUT); + pinMode(SCL, INPUT); + pinMode(DebugSwitchPin, INPUT_PULLUP); -// ------- Nav End ------- + // Motor Setup + Serial2.begin(vescbaudrate); + vescML.setSerialPort(&Serial2); + Serial3.begin(vescbaudrate); + vescMR.setSerialPort(&Serial3); + + // BLE Setup + Serial.begin(9600); + Serial1.begin(9600); + SoftSerialBLE.begin(9600); + // LoRa Setup + SoftSerialLoRa.begin(9600); -void setup() { + Serial.println("Mega Master Start!"); - pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT - pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT + debugresponse = true; + - pinMode(FOR_RELAY_PIN, OUTPUT); - pinMode(REV_RELAY_PIN, OUTPUT); + Serial.println("Adafruit LSM6DSO32 test!"); + + if (!dso32.begin_I2C()) { + // if (!dso32.begin_SPI(LSM_CS)) { + // if (!dso32.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + // Serial.println("Failed to find LSM6DSO32 chip"); + while (1) { + delay(10); + } + } - // Motor Setup - Serial2.begin(vescbaudrate); - vescML.setSerialPort(&Serial2); + Serial.println("LSM6DSO32 Found!"); + + dso32.setAccelRange(LSM6DSO32_ACCEL_RANGE_8_G); + Serial.print("Accelerometer range set to: "); + switch (dso32.getAccelRange()) { + case LSM6DSO32_ACCEL_RANGE_4_G: + Serial.println("+-4G"); + break; + case LSM6DSO32_ACCEL_RANGE_8_G: + Serial.println("+-8G"); + break; + case LSM6DSO32_ACCEL_RANGE_16_G: + Serial.println("+-16G"); + break; + case LSM6DSO32_ACCEL_RANGE_32_G: + Serial.println("+-32G"); + break; + } - Serial3.begin(vescbaudrate); - vescMR.setSerialPort(&Serial3); - - // BLE Setup - Serial.begin(9600); - Serial1.begin(9600); - SoftSerialBLE.begin(9600); + // dso32.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS ); + Serial.print("Gyro range set to: "); + switch (dso32.getGyroRange()) { + case LSM6DS_GYRO_RANGE_125_DPS: + Serial.println("125 degrees/s"); + break; + case LSM6DS_GYRO_RANGE_250_DPS: + Serial.println("250 degrees/s"); + break; + case LSM6DS_GYRO_RANGE_500_DPS: + Serial.println("500 degrees/s"); + break; + case LSM6DS_GYRO_RANGE_1000_DPS: + Serial.println("1000 degrees/s"); + break; + case LSM6DS_GYRO_RANGE_2000_DPS: + Serial.println("2000 degrees/s"); + break; + case ISM330DHCX_GYRO_RANGE_4000_DPS: + break; // unsupported range for the DSO32 + } - // LoRa Setup - SoftSerialLoRa.begin(9600); + // dso32.setAccelDataRate(LSM6DS_RATE_12_5_HZ); + Serial.print("Accelerometer data rate set to: "); + switch (dso32.getAccelDataRate()) { + case LSM6DS_RATE_SHUTDOWN: + Serial.println("0 Hz"); + break; + case LSM6DS_RATE_12_5_HZ: + Serial.println("12.5 Hz"); + break; + case LSM6DS_RATE_26_HZ: + Serial.println("26 Hz"); + break; + case LSM6DS_RATE_52_HZ: + Serial.println("52 Hz"); + break; + case LSM6DS_RATE_104_HZ: + Serial.println("104 Hz"); + break; + case LSM6DS_RATE_208_HZ: + Serial.println("208 Hz"); + break; + case LSM6DS_RATE_416_HZ: + Serial.println("416 Hz"); + break; + case LSM6DS_RATE_833_HZ: + Serial.println("833 Hz"); + break; + case LSM6DS_RATE_1_66K_HZ: + Serial.println("1.66 KHz"); + break; + case LSM6DS_RATE_3_33K_HZ: + Serial.println("3.33 KHz"); + break; + case LSM6DS_RATE_6_66K_HZ: + Serial.println("6.66 KHz"); + break; + } - Serial.println("Mega Master Start!"); + // dso32.setGyroDataRate(LSM6DS_RATE_12_5_HZ); + Serial.print("Gyro data rate set to: "); + switch (dso32.getGyroDataRate()) { + case LSM6DS_RATE_SHUTDOWN: + Serial.println("0 Hz"); + break; + case LSM6DS_RATE_12_5_HZ: + Serial.println("12.5 Hz"); + break; + case LSM6DS_RATE_26_HZ: + Serial.println("26 Hz"); + break; + case LSM6DS_RATE_52_HZ: + Serial.println("52 Hz"); + break; + case LSM6DS_RATE_104_HZ: + Serial.println("104 Hz"); + break; + case LSM6DS_RATE_208_HZ: + Serial.println("208 Hz"); + break; + case LSM6DS_RATE_416_HZ: + Serial.println("416 Hz"); + break; + case LSM6DS_RATE_833_HZ: + Serial.println("833 Hz"); + break; + case LSM6DS_RATE_1_66K_HZ: + Serial.println("1.66 KHz"); + break; + case LSM6DS_RATE_3_33K_HZ: + Serial.println("3.33 KHz"); + break; + case LSM6DS_RATE_6_66K_HZ: + Serial.println("6.66 KHz"); + break; + } + } -} +// ------- Setup Function End ------- - bool breakout; - int reason; // it switches to several other loops. - // 1 is room nav - // 2 is debug looping - // Anything else does nothing, and maybe logs an error message for now. +// ------- Loop/Main Start ------- + bool breakout; + int reason; // it switches to several other loops. + // 1 is room nav + // 2 is debug looping + // Anything else does nothing, and maybe logs an error message for now. + // After the reason is taken care of, break is reset, and the main business comes - // After the reason is taken care of, break is reset, and the main business comes + void loop() { -// --- === --- -void loop() { -// --- === --- + while (breakout == false) { // Run the user navigation -// UpdateData(); - breakout == false; - reason = 0; - - do { // Run the user navigation - - Serial.println("I am stuck in the loop"); - loopUltrasonic(); - loopIR(); - loopPressure(); - getRSSIF(); - getRSSIL(); - getRSSIR(); - if (vescML.getVescValues() ){ - Serial.print("Left RPM: "); - Serial.print(vescML.data.rpm); - Serial.print(" | Tachometer: "); - Serial.println(vescML.data.tachometerAbs); - } - else { Serial.println("Left Data Failed!"); } - - if (vescMR.getVescValues() ){ - Serial.print("Right RPM: "); - Serial.print(vescMR.data.rpm); - Serial.print(" | Tachometer: "); - Serial.println(vescMR.data.tachometerAbs); - } - else { Serial.println("Right Data Failed!"); } + Serial.println("I am stuck in the loop"); + UpdateData(); + collisionDetect(); + IMUloop(); + + if(disable()) { + breakout = true; + reason = 2; + if(debugresponse){ Serial.println("Switch over!!"); } + } + + + delay(500); + + } - setMotorSpeed(-4, -4); - delay(500); + if(reason == 1){ - } while (breakout == false); + // Navigation - switch(reason){ - case 1: // Navigation + /* + * Variable Key: + * + * Ultrasonic distance at the front: distanceUltrasonic + * Infared distance to left: distanceIRLeft + * Infared distance to right: distanceIRRight + * Left pressure sensor: pressureReadingLeft + * Right pressure sensor: pressureReadingRight + * RSSI for front: rssiF + * RSSI for left: rssiL + * RSSI for right: rssiR + * + * Format for vesc motor telemetry: [motor channel].data.[type of information] + * Motor channels (2): vescML, vescMR + * Types of information (4): rpm, inpVoltage, ampHours, tachometerAbs + * + */ -/* - * Variable Key: - * - * Ultrasonic distance at the front: distanceUltrasonic - * Infared distance to left: distanceIRLeft - * Infared distance to right: distanceIRRight - * Left pressure sensor: pressureReadingLeft - * Right pressure sensor: pressureReadingRight - * RSSI for front: rssiF - * RSSI for left: rssiL - * RSSI for right: rssiR - * - * Format for vesc motor telemetry: [motor channel].data.[type of information] - * Motor channels (2): vescML, vescMR - * Types of information (4): rpm, inpVoltage, ampHours, tachometerAbs - * - */ + + bool targetreached = false; + while(targetreached == false){ delay(500); } + } - - bool targetreached = false; - while(targetreached == false){ delay(500); } - break; + + else if(reason == 2) { // Debugging + while(disable() == 1){ + Serial.println("Debug mode!"); + //Debugger(); + delay(1000); + } + Serial.println("Get out of debug!"); + } - case 2: // Debugging - while(1){ - Debugger(); - delay(500); - // stop whenever?? + else{ // What?? + Serial.println("How did we get here?"); + delay(1000); + Serial.println("Short timeout for you."); + delay(5000); } - break; + if(debugresponse){ Serial.println("Back to the top!"); } + breakout = false; + reason = 0; - default: // What?? - // Log error message - delay(500); - break; + } -// --- === --- -} } -// --- === --- +// ------- Loop/Main End ------- \ No newline at end of file From dc191dd673641504102356a905eadbfa10adfc5d Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 7 May 2023 18:54:55 -0700 Subject: [PATCH 10/16] Limit and Speakers and More! All sorts of stuff happening on 5/7. Demo movement works pretty consistently! --- CW_Motor.ino => CW_Motor/CW_Motor.ino | 0 Mega-edition/Mega_Master/Mega_Master.ino | 193 ++++++++++++++--------- 2 files changed, 121 insertions(+), 72 deletions(-) rename CW_Motor.ino => CW_Motor/CW_Motor.ino (100%) diff --git a/CW_Motor.ino b/CW_Motor/CW_Motor.ino similarity index 100% rename from CW_Motor.ino rename to CW_Motor/CW_Motor.ino diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index d646a10..12d5a22 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -1,5 +1,8 @@ + + + /* - * CORRECT EDITION 4/23/23 + * CORRECT EDITION 5/7/23 */ @@ -13,37 +16,13 @@ */ /* --- Latest Update Log --- - - Date: 4/23/23 - - Alex. Adding the debug button. It puts it in debug / disable ! - - Date: 4/16/23 - - Alex. Trying to add Vesc data. - - Date: 4/2/23 - - Alex. We're doing this thing. - - Expand the RSSI function to work with all three serial ports. - - Isolate redundant telemetry functions. Comment out until it's verified the code works: then remove. - - Incorporate actuators? into hardware scheme. - - Start on Leo's nav algorithm. - - Date: 3/19/23 - - Alex. Doing channel allocations. - - Date: 3/15/23 - - Alex here! Just finished rewiring this thing for the Mega. Wiped out all the UART communication save for BLE, and brought over all the motor control stuff, now attached to hardware serials. - - IMPORTANT THING: we have 3 BLE sensors we should be tracking, and this program's telemetry just accounts for 1! - - The other two telemetry updates should go in the bottom, next to the first. - - Remember that two are hardware serials and one is a software serial! - * Should the telemetries all update at once, or should they stagger over a period? - * Could we expand the existing UART framework to accomodate commands from the 3 sensors? I.E. a case switcher that checks Purpose byte, and each BLE gets its own Purpose number -*/ + * + * Alex here. Dealing with several things + * + * + * + * + */ // ------- Setup Start ------- @@ -52,6 +31,8 @@ #include #include #include + + // Constants bool debugresponse = true; @@ -68,12 +49,6 @@ int rssiF = 0; int rssiL = 0; int rssiR = 0; - - - bool disable() { - return (!digitalRead(22) ); - } - // Pins // Analog @@ -84,12 +59,30 @@ // Digital and Serial - const int UltrasonicTrig = 27; - const int UltrasonicEcho = 28; - const int FOR_RELAY_PIN = 3; - const int REV_RELAY_PIN = 4; + const int LidarPin = 10; + + const int UltrasonicTrig = 28; + const int UltrasonicEcho = 29; + + const int RelayLF = 32; + const int RelayLR = 33; + const int RelayRF = 34; + const int RelayRR = 35; + const int RelayBF = 36; + const int RelayBR = 37; + + const int BumperPin = 40; + const int SpeakerPin = 41; + + const int DebugSwitchPin = 49; + + bool disable() { + return (!digitalRead(DebugSwitchPin) ); + } - const int DebugSwitchPin = 22; + bool bumped() { + return (!digitalRead(BumperPin) ); + } /* All parenthesis in rx -> tx order - Serial (0, 1) - Serial1 (19, 18) - Serial2 (17, 16) - Serial3 (15, 14) @@ -99,14 +92,15 @@ BLE Sensors: No. 1: Serial0 (Front) No. 2: Serial1 (Right) - No. 3: Bonus software serial (22, 23) (Left) + No. 3: Bonus software serial (52, 53) (Left) LoRa: - Software serial (30,31) + Software serial (50,51) + + Only software ser ial pins have to be specified. All others are set by default by Arduino */ - Only software serial pins have to be specified. All others are set by default by Arduino */ - - SoftwareSerial SoftSerialBLE(52, 53); SoftwareSerial SoftSerialLoRa(50, 51); + SoftwareSerial SoftSerialBLE(52, 53); + // ------- Setup End ------- @@ -196,13 +190,17 @@ // ------- Actuator Start ------- void extendActuators(){ - digitalWrite(FOR_RELAY_PIN, HIGH); - digitalWrite(REV_RELAY_PIN, LOW); + digitalWrite(RelayLF, HIGH); + digitalWrite(RelayRF, HIGH); + digitalWrite(RelayLR, LOW); + digitalWrite(RelayRR, LOW); } void retractActuators(){ - digitalWrite(FOR_RELAY_PIN, LOW); - digitalWrite(REV_RELAY_PIN, HIGH); + digitalWrite(RelayLR, HIGH); + digitalWrite(RelayRR, HIGH); + digitalWrite(RelayLF, LOW); + digitalWrite(RelayRF, LOW); } // ------- Actuator End ------- @@ -470,14 +468,19 @@ // ------- Bluetooth Serial End ------- + + // ------- LoRa Start ------- // ------- LoRa End ------- + + // ------- IMU Start ------- - // Basic demo for accelerometer & gyro readings from Adafruit - // LSM6DSO32 sensor - #include + + + // Basic demo for accelerometer & gyro readings from Adafruit + // LSM6DSO32 sensor added // For SPI mode, we need a CS pin #define LSM_CS 12 @@ -492,7 +495,7 @@ void IMUloop() { - // /* Get a new normalized sensor event */ + // Get a new normalized sensor event sensors_event_t accel; sensors_event_t gyro; sensors_event_t temp; @@ -506,7 +509,7 @@ // Serial.print(temp.temperature); // Serial.println(" deg C"); - /* Display the results (acceleration is measured in m/s^2) */ + // Display the results (acceleration is measured in m/s^2) // Serial.print("\t\tAccel X: "); Serial.print(accel.acceleration.x); Serial.print("\t\tAccel X Tared: "); @@ -530,7 +533,7 @@ Serial.print(distance_x); Serial.println(" m "); - // /* Display the results (rotation is measured in rad/s) */ + // // Display the results (rotation is measured in rad/s) // // Serial.print("\t\tGyro X: "); // Serial.print(gyro.gyro.x); // Serial.print(" \tY: "); @@ -558,18 +561,24 @@ // Serial.println(); // delayMicroseconds(10000); } + + + // ------- IMU End ------- + + // ------- Control Start ------- void UpdateData(){ - // loopUltrasonic(); - // loopIR(); - // loopPressure(); - // getRSSIF(); - // getRSSIL(); - // getRSSIR(); - // motordebug(); + loopUltrasonic(); + loopIR(); + loopPressure(); + getRSSIF(); + getRSSIL(); + getRSSIR(); + motordebug(); + IMUloop(); } int threshUltrasonic = 60; // (reading of about 5 inches) @@ -607,14 +616,19 @@ // ------- Nav Start ------- + void collisionDetect() { - if (distanceUltrasonic > 30) { + if (distanceUltrasonic > 80) { setMotorSpeed(-5,-5); + Serial.println("CollisionDetect - Driving!"); } else { setMotorSpeed(0,0); + Serial.println("CollisionDetect - Stopped!"); } } + + // ------- Nav End ------- // ------- Setup Function Start ------- @@ -624,17 +638,26 @@ pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT - pinMode(FOR_RELAY_PIN, OUTPUT); - pinMode(REV_RELAY_PIN, OUTPUT); + pinMode(RelayLF, OUTPUT); + pinMode(RelayLR, OUTPUT); + pinMode(RelayRF, OUTPUT); + pinMode(RelayRR, OUTPUT); + pinMode(RelayBF, OUTPUT); + pinMode(RelayBR, OUTPUT); - pinMode(LeftInfared, INPUT); - pinMode(RightInfared, INPUT); + // pinMode(LeftInfared, INPUT); + // pinMode(RightInfared, INPUT); pinMode(SDA, INPUT); pinMode(SCL, INPUT); + pinMode(SpeakerPin, OUTPUT); + + pinMode(BumperPin, INPUT_PULLUP); + pinMode(DebugSwitchPin, INPUT_PULLUP); + // Motor Setup Serial2.begin(vescbaudrate); vescML.setSerialPort(&Serial2); @@ -782,6 +805,23 @@ Serial.println("6.66 KHz"); break; } + + + Serial.println("Acutator test"); + digitalWrite(RelayLR, HIGH); + delay(1000); + digitalWrite(RelayLR, LOW); + delay(1000); + digitalWrite(RelayLF, HIGH); + delay(1000); + digitalWrite(RelayLF, LOW); + delay(1000); + + + + + Serial.println("Setup complete!"); + } // ------- Setup Function End ------- @@ -796,13 +836,14 @@ void loop() { + while (breakout == false) { // Run the user navigation Serial.println("I am stuck in the loop"); UpdateData(); collisionDetect(); - IMUloop(); + if(disable()) { breakout = true; reason = 2; @@ -851,6 +892,14 @@ while(disable() == 1){ Serial.println("Debug mode!"); //Debugger(); + + if(bumped()){ + Serial.println("Yes bumper!"); + } + else { + Serial.println("No bumper!"); + } + delay(1000); } Serial.println("Get out of debug!"); @@ -872,4 +921,4 @@ } -// ------- Loop/Main End ------- \ No newline at end of file +// ------- Loop/Main End ------- From e117640ca7bc33b767a45f92dfc9cce708ab747d Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 14 May 2023 17:43:28 -0700 Subject: [PATCH 11/16] Funny Dev Change We do rssi reading in debug --- .DS_Store | Bin 6148 -> 6148 bytes Mega-edition/Mega_Master/Mega_Master.ino | 10 +++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/.DS_Store b/.DS_Store index bbe858b3b45cd4d5c729eeaae5810ccdf90ef28a..17066f9f4cc33fb8c81cda00317d1cbf1c530209 100644 GIT binary patch delta 31 ncmZoMXfc@J&&aniU^g=(-(((^iJPyo7%@+*wAjqf@s}R}pw$X0 delta 70 zcmZoMXfc@J&&a Date: Sat, 27 May 2023 20:38:01 -0700 Subject: [PATCH 12/16] Demo code created --- Mega-edition/Mega_Master/Mega_Master.ino | 115 ++++++++++++++++++++++- 1 file changed, 110 insertions(+), 5 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 91a27cb..cbf7c98 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -570,6 +570,10 @@ // ------- Control Start ------- + int RSSILavg; + int RSSIRavg; + int RSSIFavg; + void UpdateData(){ loopUltrasonic(); loopIR(); @@ -579,6 +583,8 @@ getRSSIR(); motordebug(); IMUloop(); + + } int threshUltrasonic = 60; // (reading of about 5 inches) @@ -612,6 +618,8 @@ } } + + void RSSIPlotter(){} // ------- Control End ------- // ------- Nav Start ------- @@ -627,6 +635,100 @@ } } + // float mapRSSI( float x ) { + // const int expecthigh = 80; + // const int expectlow = 50; + + // return (x - expectlow) * 100 / (expecthigh - expectlow); + // } + + + // PROBLEM. Cannot use one mapper function for multiple + // float mapRSSI (float x) { // RSSI with input smoothing and basic adaptive mapping + // static float expecthigh = 80; // upper boundary + // static float expectlow = 50; // lower boundary + // static float cumulativeavg = 0; // buffer for input smoothing + // float end; // result variable + + // cumulativeavg = (cumulativeavg + x) / 2; // smooth input with basic integral functionss + + // if (cumulativeavg > expecthigh){ + // end = 1; + // expecthigh = cumulativeavg; + // } + // else if (cumulativeavg < expectlow){ + // end = 0; + // expectlow = cumulativeavg; + // } + // else{ + // end = (cumulativeavg - expectlow) / (expecthigh - expectlow); + // } + + // return (end); + // } + + static float ehL = 80; // Expected Highest. adaptive upper bound + static float elL = 50; // Expected Lowest. adaptivel lower bound + static float caL = 60; // Cumulative Average. + + static float ehR = 80; + static float elR = 50; + static float caR = 60; + + float mapRSSIL () { // Outputs 0-1. Input smoothing and advanced bound manipulation. + + float end; // result variable + + caL = (caL + rssiL) / 2; // smooth input with basic integral function + + + if (caL > ehL){ // If it exceeds highest bound... + end = 1; // + ehL = caL; + elL++; // Since it's not being touched, bring lower bound up. + } + else if(caL < elL){ + end = 0; + elL = caL; + ehL--; + } + else{ + end = (caL - elL) / (ehL - elL); + ehL--; elL++; + } + + return (end); + } + + float mapRSSIR () { // OUTPUTS ON 0-1 SCALE + float end; + + caR = (caR + rssiR) / 2; + + if (caR > ehR){ + end = 1; + ehR = caR; + elR++; + } + else if(caR < elR){ + end = 0; + elR = caR; + ehR--; + } + else{ + end = (caR - elR) / (ehR - elR); + ehR--; elR++; + } + + return (end); + } + + void turnToBeacon() { + float angle = mapRSSIR - mapRSSIL; + setMotorSpeed( angle * 5, -angle * 5); + + } + // ------- Nav End ------- @@ -678,12 +780,14 @@ debugresponse = true; + + Serial.println("Adafruit LSM6DSO32 test!"); if (!dso32.begin_I2C()) { - // if (!dso32.begin_SPI(LSM_CS)) { - // if (!dso32.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { - // Serial.println("Failed to find LSM6DSO32 chip"); + if (!dso32.begin_SPI(LSM_CS)) { + if (!dso32.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + Serial.println("Failed to find LSM6DSO32 chip"); } } while (1) { delay(10); } @@ -839,9 +943,9 @@ while (breakout == false) { // Run the user navigation - Serial.println("I am stuck in the loop"); + Serial.println("I am in the main loop!"); UpdateData(); - collisionDetect(); + turnToBeacon(); if(disable()) { @@ -872,6 +976,7 @@ * RSSI for front: rssiF * RSSI for left: rssiL * RSSI for right: rssiR + * (USE mapRSSI() or mapRSSIbetter() to turn these into 0-1 scaled functions for accurate relative strength) * * Format for vesc motor telemetry: [motor channel].data.[type of information] * Motor channels (2): vescML, vescMR From 00e30a2863cc11dce6f770dd2ecbf755f4ed5ad0 Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sun, 28 May 2023 19:20:42 -0700 Subject: [PATCH 13/16] RSSI PLotting we idd it --- Mega-edition/Mega_Master/Mega_Master.ino | 128 ++++++++++++++++++----- 1 file changed, 99 insertions(+), 29 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index cbf7c98..09ce5dc 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -2,7 +2,7 @@ /* - * CORRECT EDITION 5/7/23 + * CORRECT EDITION 5/28/23 */ @@ -17,7 +17,7 @@ /* --- Latest Update Log --- * - * Alex here. Dealing with several things + * Alex here. Dealing with debug garbage. * * * @@ -102,6 +102,11 @@ SoftwareSerial SoftSerialBLE(52, 53); + void debugrespond ( String text ) { + if(debugresponse){ Serial.println(text); } + } + + // ------- Setup End ------- // ------- Motor Start ------- @@ -334,6 +339,8 @@ SoftSerialBLE.listen(); //find serial value + if (SoftSerialBLE.available() == 0 && debugresponse) { Serial.println("GetRSSIL - Failed!"); } else {} + while (SoftSerialBLE.available() > 0) { //If there are available bytes... static unsigned int tlm_pos = 0; //start at index zero @@ -380,6 +387,8 @@ void getRSSIR() { + if (Serial1.available() == 0 && debugresponse) { Serial.println("GetRSSIR - Failed!"); } else {} + while (Serial1.available() > 0) { //If there are available bytes... static unsigned int tlm_pos = 0; //start at index zero @@ -425,6 +434,8 @@ void getRSSIF() { + if (Serial.available() == 0 && debugresponse) { Serial.println("GetRSSIF - Failed!"); } else {} + while (Serial.available() > 0) { //If there are available bytes... static unsigned int tlm_pos = 0; //start at index zero @@ -510,15 +521,18 @@ // Serial.println(" deg C"); // Display the results (acceleration is measured in m/s^2) // + if (debugresponse) { Serial.print("\t\tAccel X: "); Serial.print(accel.acceleration.x); Serial.print("\t\tAccel X Tared: "); + } if (abs(accel.acceleration.x) > 0.2){ accel_x_tared = accel.acceleration.x + accel_x_tare; } else { accel_x_tared = accel.acceleration.x; } - Serial.print(accel_x_tared); + + if (debugresponse) { Serial.print(accel_x_tared); } if (abs(accel_x_tared) >= 0.2){ @@ -528,10 +542,12 @@ // Serial.print(accel.acceleration.y); // Serial.print(" \tZ: "); // Serial.print(accel.acceleration.z); + if (debugresponse) { Serial.println(" m/s^2 "); Serial.print(distance_x); Serial.println(" m "); + } // // Display the results (rotation is measured in rad/s) // // Serial.print("\t\tGyro X: "); @@ -635,12 +651,7 @@ } } - // float mapRSSI( float x ) { - // const int expecthigh = 80; - // const int expectlow = 50; - - // return (x - expectlow) * 100 / (expecthigh - expectlow); - // } + // PROBLEM. Cannot use one mapper function for multiple @@ -685,16 +696,16 @@ if (caL > ehL){ // If it exceeds highest bound... end = 1; // ehL = caL; - elL++; // Since it's not being touched, bring lower bound up. + //elL++; // Since it's not being touched, bring lower bound up. } else if(caL < elL){ end = 0; elL = caL; - ehL--; + //ehL--; } else{ end = (caL - elL) / (ehL - elL); - ehL--; elL++; + //ehL--; elL++; } return (end); @@ -708,16 +719,16 @@ if (caR > ehR){ end = 1; ehR = caR; - elR++; + //elR++; } else if(caR < elR){ end = 0; elR = caR; - ehR--; + //ehR--; } else{ end = (caR - elR) / (ehR - elR); - ehR--; elR++; + //ehR--; elR++; } return (end); @@ -737,6 +748,10 @@ void setup() { + + debugresponse = false; + + pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT @@ -775,12 +790,10 @@ // LoRa Setup SoftSerialLoRa.begin(9600); - Serial.println("Mega Master Start!"); - - debugresponse = true; - + debugrespond("Mega Master Start!"); +/* Serial.println("Adafruit LSM6DSO32 test!"); @@ -910,8 +923,10 @@ break; } + */ + - Serial.println("Acutator test"); + debugrespond("Actuator test"); digitalWrite(RelayLR, HIGH); delay(1000); digitalWrite(RelayLR, LOW); @@ -924,7 +939,7 @@ - Serial.println("Setup complete!"); + debugrespond("Setup complete"); } @@ -938,24 +953,48 @@ // Anything else does nothing, and maybe logs an error message for now. // After the reason is taken care of, break is reset, and the main business comes + void loop() { + static int im = 0; while (breakout == false) { // Run the user navigation - Serial.println("I am in the main loop!"); + debugrespond("----------------------"); + debugrespond("I am in the main loop!"); UpdateData(); - turnToBeacon(); + //turnToBeacon(); + + + + + +// +// if (im == 10){ +// setMotorSpeed (0, 0); +// } +// else if (im == 20){ +// setMotorSpeed (0, 0); +// im = 0; +// } +// else if(im == 5){ +// setMotorSpeed(5,5); +// } +// else if(im == 15){ +// setMotorSpeed(-5, -5); +// } +// im++; if(disable()) { breakout = true; reason = 2; - if(debugresponse){ Serial.println("Switch over!!"); } + debugrespond("Serial, switch over!"); } + + - - delay(500); + delay(100); } @@ -994,14 +1033,44 @@ else if(reason == 2) { // Debugging + debugrespond("Debug mode!"); + debugrespond(""); + while(disable() == 1){ - Serial.println("Debug mode!"); //Debugger(); - + getRSSIL(); getRSSIF(); getRSSIR(); + static int cumRSSIL = 0; + static int cumRSSIR = 0; + static int oldcRSSIL; + static int oldcRSSIR; + static int driveangle = 0; + + if (rssiL < -10) { cumRSSIL = (cumRSSIL + rssiL) / 2; } else {} + if (rssiR < -10) { cumRSSIR = (cumRSSIR + rssiR) / 2; } else {} + + driveangle = (driveangle + (cumRSSIR - cumRSSIL) / 2) / 2; + + //Serial.print("raw L: "); + // Serial.print(rssiL); + // Serial.print(","); + + //Serial.print("averaged L: "); + Serial.print(cumRSSIL); + Serial.print(","); + + //Serial.print("raw R: "); + // Serial.print(rssiR); + // Serial.print(","); + + //Serial.print("averaged R: "); + Serial.print(cumRSSIR); + Serial.print(","); + + Serial.println(driveangle); /* if(bumped()){ @@ -1013,7 +1082,7 @@ */ - delay(250); + delay(200); } Serial.println("Get out of debug!"); } @@ -1032,6 +1101,7 @@ reason = 0; + Serial.println("----------------------"); } // ------- Loop/Main End ------- From 0da8103a62d3cb347c4b15072aae2c7ffb2abcbd Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sat, 3 Jun 2023 22:51:23 -0700 Subject: [PATCH 14/16] Crispening up the demo Getting a framework of demo code --- Mega-edition/Mega_Master/Mega_Master.ino | 195 +++++++---------------- 1 file changed, 61 insertions(+), 134 deletions(-) diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index 09ce5dc..3f9e15e 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -76,7 +76,7 @@ const int DebugSwitchPin = 49; - bool disable() { + bool debugdisable() { return (!digitalRead(DebugSwitchPin) ); } @@ -208,6 +208,15 @@ digitalWrite(RelayRF, LOW); } + void brake(int state){ //placehlolder for solenoid brakes on the walker + if(state == 1){ + + } + else{ + + } + } + // ------- Actuator End ------- // ------- Sensor Start ------- @@ -293,33 +302,12 @@ //----- - String pad3(int input) { // shoves a few zeros on the head of a number - if (input < 100) { - if (input < 10) { - return "00" + String(input); - } else { - return "0" + String(input); - } - } else { - return String(input); - } - } - - int between(int input, int low, int high){ // self explanatory if ( input > high ) { return high; } else if ( input < low ) { return low; } else { return input; } } - - void listBytes() { // lists the individual bytes of a message - for (int i = 0; i < (maxlength - 1); i++) { - String indivbyte; - indivbyte = "Byte No. " + String(i) + " Is " + telemetry[i]; - Serial.println(indivbyte); - } - } /* RSSI Functions: - Simply run em' once in the update cycle (ideally) @@ -481,15 +469,8 @@ -// ------- LoRa Start ------- -// ------- LoRa End ------- - - - // ------- IMU Start ------- - - // Basic demo for accelerometer & gyro readings from Adafruit // LSM6DSO32 sensor added @@ -599,14 +580,10 @@ getRSSIR(); motordebug(); IMUloop(); - - } - - int threshUltrasonic = 60; // (reading of about 5 inches) void Debugger(){ - + /* const int maxspeed = 6; static int Speed = 0; @@ -614,7 +591,7 @@ int target = 0; // Target Shifting - if ( distanceUltrasonic > threshUltrasonic ) { // Anything to stop both motors + if ( distanceUltrasonic > Ultrasonic ) { // Anything to stop both motors target = maxspeed; } @@ -630,8 +607,9 @@ Serial.print (" , Motor duty is set to "); Serial.println (Speed); Serial.println (""); - } + + */ } @@ -651,33 +629,6 @@ } } - - - - // PROBLEM. Cannot use one mapper function for multiple - // float mapRSSI (float x) { // RSSI with input smoothing and basic adaptive mapping - // static float expecthigh = 80; // upper boundary - // static float expectlow = 50; // lower boundary - // static float cumulativeavg = 0; // buffer for input smoothing - // float end; // result variable - - // cumulativeavg = (cumulativeavg + x) / 2; // smooth input with basic integral functionss - - // if (cumulativeavg > expecthigh){ - // end = 1; - // expecthigh = cumulativeavg; - // } - // else if (cumulativeavg < expectlow){ - // end = 0; - // expectlow = cumulativeavg; - // } - // else{ - // end = (cumulativeavg - expectlow) / (expecthigh - expectlow); - // } - - // return (end); - // } - static float ehL = 80; // Expected Highest. adaptive upper bound static float elL = 50; // Expected Lowest. adaptivel lower bound static float caL = 60; // Cumulative Average. @@ -690,11 +641,11 @@ float end; // result variable - caL = (caL + rssiL) / 2; // smooth input with basic integral function + caL = (caL + abs(rssiL)) / 2; // smooth input with basic integral function if (caL > ehL){ // If it exceeds highest bound... - end = 1; // + end = 1; ehL = caL; //elL++; // Since it's not being touched, bring lower bound up. } @@ -714,7 +665,7 @@ float mapRSSIR () { // OUTPUTS ON 0-1 SCALE float end; - caR = (caR + rssiR) / 2; + caR = (caR + abs(rssiR)) / 2; if (caR > ehR){ end = 1; @@ -735,7 +686,7 @@ } void turnToBeacon() { - float angle = mapRSSIR - mapRSSIL; + float angle = mapRSSIR() - mapRSSIL(); setMotorSpeed( angle * 5, -angle * 5); } @@ -749,7 +700,7 @@ void setup() { - debugresponse = false; + debugresponse = true; pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT @@ -945,6 +896,8 @@ // ------- Setup Function End ------- + + // ------- Loop/Main Start ------- bool breakout; int reason; // it switches to several other loops. @@ -955,55 +908,33 @@ void loop() { - static int im = 0; - - - while (breakout == false) { // Run the user navigation - - debugrespond("----------------------"); - debugrespond("I am in the main loop!"); + + while (breakout == false) { // Run the user navigation + + if(debugresponse){ Serial.println("----------------------");} UpdateData(); - //turnToBeacon(); - - - - - -// -// if (im == 10){ -// setMotorSpeed (0, 0); -// } -// else if (im == 20){ -// setMotorSpeed (0, 0); -// im = 0; -// } -// else if(im == 5){ -// setMotorSpeed(5,5); -// } -// else if(im == 15){ -// setMotorSpeed(-5, -5); -// } -// im++; - - - if(disable()) { + // Autobrake feature + if (pressureReadingLeft >= 150 || pressureReadingRight >= 150) { brake(1); } else { brake(0); } + + if(debugdisable()) { breakout = true; - reason = 2; - debugrespond("Serial, switch over!"); + reason = 1; // FOR NOW, DEBUG SWITCH SENDS IT TO AUTON + if(debugresponse){ Serial.println("I am in the main loop!"); } } delay(100); - } + } if(reason == 1){ + if(debugresponse){ Serial.println("Begin Navigation"); } // Navigation - + /* * Variable Key: * @@ -1023,10 +954,30 @@ * */ - bool targetreached = false; - while(targetreached == false){ delay(500); } + while(targetreached == false){ + const float kdrive = 0.2; + + static int cumRSSIL = 0; + static int cumRSSIR = 0; + static int oldcRSSIL; + static int oldcRSSIR; + static int driveangle = 0; + + if (rssiL < -10) { cumRSSIL = (cumRSSIL + rssiL) / 2; } else {} + if (rssiR < -10) { cumRSSIR = (cumRSSIR + rssiR) / 2; } else {} + + driveangle = (driveangle + (cumRSSIR - cumRSSIL) / 2); + + if (distanceUltrasonic >= 50) { setMotorSpeed(-driveangle*kdrive, driveangle*kdrive); } + else{ setMotorSpeed(0,0); } + + targetreached = !debugdisable(); + delay(100); + + } + debugrespond("Done with auton!"); } @@ -1036,8 +987,8 @@ debugrespond("Debug mode!"); debugrespond(""); - while(disable() == 1){ - //Debugger(); + while(debugdisable() == 1){ + getRSSIL(); getRSSIF(); @@ -1053,35 +1004,10 @@ driveangle = (driveangle + (cumRSSIR - cumRSSIL) / 2) / 2; - //Serial.print("raw L: "); - // Serial.print(rssiL); - // Serial.print(","); - - //Serial.print("averaged L: "); Serial.print(cumRSSIL); Serial.print(","); - - //Serial.print("raw R: "); - // Serial.print(rssiR); - // Serial.print(","); - - //Serial.print("averaged R: "); - Serial.print(cumRSSIR); - Serial.print(","); - - - Serial.println(driveangle); - /* - - if(bumped()){ - Serial.println("Yes bumper!"); - } - else { - Serial.println("No bumper!"); - } - - */ - + Serial.println(cumRSSIR); + delay(200); } Serial.println("Get out of debug!"); @@ -1102,6 +1028,7 @@ Serial.println("----------------------"); + Serial.println(""); Serial.println(""); } // ------- Loop/Main End ------- From 3696b1b4aaf882869dacca46151dda21daf2a7da Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sat, 10 Jun 2023 13:27:02 -0700 Subject: [PATCH 15/16] Added itsy code file finally --- .DS_Store | Bin 6148 -> 8196 bytes niceItsyCode/niceItsyCode.ino | 317 ++++++++++++++++++++++++++++++++++ 2 files changed, 317 insertions(+) create mode 100644 niceItsyCode/niceItsyCode.ino diff --git a/.DS_Store b/.DS_Store index 17066f9f4cc33fb8c81cda00317d1cbf1c530209..133eef2be8f20a78de244fabd14d37c2537af8f0 100644 GIT binary patch delta 796 zcmZoMXmOBWU|?W$DortDU;r^WfEYvza8E20o2aMAD84aZH}hr%jz7$c**Q2SHn1>? zPv&8n7|Frl%n;5H&*00D&rkxyMV>kN$w@i+Ng&NYBV~cK-hVIvvKSb6(A6{OF=R62 zG32A`+uXtgg<=&BeUsx@ z`#B8F4RsWZEet1rWp%E{p)e=iFgQ6sw*c&N2!Ycvx%nAB&ODi4#B;Jg4+jU(CQ!0rm>kbD4OfUF` +#include +#include +#include +#include + +#define VERBOSE_OUTPUT (0) // Set this to 1 for verbose adv packet output to the serial monitor +#define ARRAY_SIZE (2) // The number of RSSI values to store and compare +#define TIMEOUT_MS (2500) // Number of milliseconds before a record is invalidated in the list + +#if (ARRAY_SIZE <= 1) + #error "ARRAY_SIZE must be at least 2" +#endif +#if (ENABLE_TFT) && (ENABLE_OLED) + #error "ENABLE_TFT and ENABLE_OLED can not both be set at the same time" +#endif + +// Custom UUID used to differentiate this device. +// Use any online UUID generator to generate a valid UUID. +// Note that the byte order is reversed ... CUSTOM_UUID +// below corresponds to the follow value: +// df67ff1a-718f-11e7-8cf7-a6006ad3dba0 +// const uint8_t CUSTOM_UUID[] = +// { +// 0xA0, 0xDB, 0xD3, 0x6A, 0x00, 0xA6, 0xF7, 0x8C, +// 0xE7, 0x11, 0x8F, 0x71, 0x1A, 0xFF, 0x67, 0xDF +// }; + +// 426c7565-4368-6172-6d42-6561636f6e73 +const uint8_t CUSTOM_UUID[] = +{ + 0x73, 0x6E, 0x6F, 0x63, 0x61, 0x65, 0x42, 0x6D, + 0x72, 0x61, 0x68, 0x43, 0x65, 0x75, 0x6C, 0x42 +}; + +//// BLE Beacon +//const uint8_t CUSTOM_MAC[] = +//{ +// 0xD6, 0xDD, 0x06, 0x02, 0x34, 0xDD +//}; + + + +//// BLE Beacon +const uint8_t CUSTOM_MAC[] = +{ + 0x6B, 0xE2, 0x06, 0x02, 0x34, 0xDD +}; + + + +//// BLE Beacon (wearable) +// const uint8_t CUSTOM_MAC[] = +// { +// 0x7B, 0x9B, 0xFE, 0xEC, 0x60, 0x02 +// }; + + + +//// Mr. Leo's Phone +//const uint8_t CUSTOM_MAC[] = +//{ +// 0x2E, 0x18, 0x4D, 0xF0, 0xD0, 0xF7 +//}; + +// Mr. Leo's Computer +// uint8_t CUSTOM_MAC[] = +// { +// 0x68, 0x47, 0x3E, 0x4E, 0xE6, 0x7D +// }; + +BLEUuid uuid = BLEUuid(CUSTOM_UUID); +BLEUuid ble_uuid = BLEUuid(CUSTOM_UUID); + +boolean return_of_the_mac = false; +boolean missing_mac = true; + +// Analog +//int ANALOG0 = 14; +//int ANALOG1 = 15; +//int ANALOG2 = 16; +//int ANALOG3 = 17; +//int ANALOG4 = 18; +//int ANALOG5 = 19; + +// Digital +//int DIGITAL2 = 2; +//int DIGITAL5 = 5; +//int DIGITAL7 = 7; +//int DIGITAL9 = 9; +//int DIGITAL10 = 10; +//int DIGITAL11 = 11; +int MicroCtrlMaster_rx = 12; //rx is purple +int MicroCtrlMaster_tx = 13; //tx is white + +SoftwareSerial MicroCtrlMaster(MicroCtrlMaster_rx,MicroCtrlMaster_tx); +// Arduino Pin 12 goes to MicroCtrlMaster TX +// Arduino Pin 13 goes to MicroCtrlMaster RX + +/* This struct is used to track detected nodes */ +typedef struct node_record_s +{ + uint8_t addr[6]; // Six byte device address + int8_t rssi; // RSSI value + uint32_t timestamp; // Timestamp for invalidation purposes + int8_t reserved; // Padding for word alignment +} node_record_t; + +node_record_t records[ARRAY_SIZE]; + +String pad4( int input ){ + + if ( input < 10) { + return "000" + String(input); + } + if ( input < 100) { + return "00" + String(input); + } + if ( input < 1000) { + return "0" + String(input); + } + return String(input); + +} + +String pad3( int input ){ + + if ( input < 10) { + return "00" + String(input); + } + if ( input < 100) { + return "0" + String(input); + } + return String(input); + +} + + +void sendRSSITelem(int rssi_telem){ + String cmd; // initialize + + // cmd = "84-" + pad3(abs(int(round(rssi_telem))) ); + cmd = rssi_telem; + // cmd = "84-100"; + // PL + RSSI + + // Adafruit hates writing strings over serial, so here we handconvert string into char array + char charcommand[cmd.length()]; + + for (int i = 0; i < cmd.length(); i++){ + charcommand[i] = cmd.charAt(i); + } + MicroCtrlMaster.write(charcommand); + MicroCtrlMaster.println(" "); + + Serial.print("Command is: "); + Serial.println(charcommand); //DEBUG +} + + +void setup() +{ + Serial.begin(115200); + while ( !Serial ) delay(10); // for nrf52840 with native usb + + MicroCtrlMaster.begin(9600); + + Serial.println("Bluefruit52 Central Proximity Example"); + Serial.println("-------------------------------------\n"); + + + /* Clear the results list */ + memset(records, 0, sizeof(records)); + for (uint8_t i = 0; i= -80 dBm + Bluefruit.Scanner.setInterval(160, 80); // in units of 0.625 ms + Bluefruit.Scanner.useActiveScan(true); // Request scan response data + Bluefruit.Scanner.start(0); // 0 = Don't stop scanning after n seconds + Serial.println("Scanning ..."); +} + +/* This callback handler is fired every time a valid advertising packet is detected */ +void scan_callback(ble_gap_evt_adv_report_t* report) +{ + node_record_t record; + + /* Prepare the record to try to insert it into the existing record list */ + memcpy(record.addr, report->peer_addr.addr, 6); /* Copy the 6-byte device ADDR */ + record.rssi = report->rssi; /* Copy the RSSI value */ + record.timestamp = millis(); /* Set the timestamp (approximate) */ + + return_of_the_mac = false; + /* Attempt to insert the record into the list */ +// if (insertRecord(&record) == 1) /* Returns 1 if the list was updated */ + if (true) + { + if (memcmp(CUSTOM_MAC, report->peer_addr.addr, 6)==0) + { + //DEBUG + // Serial.printBuffer(report->peer_addr.addr, 6); + // Serial.println(""); + // Serial.print("RETURN OF THE MAC"); + // Serial.println(""); + return_of_the_mac = true; + missing_mac = false; + } else { + return_of_the_mac = false; + } + + } + + + + +/* Fully parse and display the advertising packet to the Serial Monitor + * if verbose/debug output is requested */ +if (VERBOSE_OUTPUT | return_of_the_mac | missing_mac){ + uint8_t len = 0; + uint8_t buffer[32]; + memset(buffer, 0, sizeof(buffer)); + + /* Display the timestamp and device address */ + // if (report->type.scan_response) //DEBUG + // { + // Serial.printf("[SR%10d] Packet received from ", millis()); + // } + // else //DEBUG + // { + // Serial.printf("[ADV%9d] Packet received from ", millis()); + + // } + // MAC is in little endian --> print reverse + // Serial.printBufferReverse(report->peer_addr.addr, 6, ':'); + // Serial.println(""); + + /* RSSI value */ + // Serial.printf("%14s %d dBm\n", "MY RSSI", report->rssi); //DEBUG + Serial.printf("%d \n", report->rssi); + + + // if (missing_mac == false & return_of_the_mac == true){ + if (return_of_the_mac == true){ + sendRSSITelem(report->rssi); + + } + + // if (report->rssi > -35 & missing_mac== true){ + // Serial.println(""); + // Serial.print("THE NEW MAC DADDY"); + // Serial.println(""); + // Serial.print(report->rssi); + // Serial.println(""); + // Serial.printBufferReverse(report->peer_addr.addr, 6, ':'); + // Serial.println(""); + + // memcpy(CUSTOM_MAC, report->peer_addr.addr, 6); + // Serial.printBufferReverse(CUSTOM_MAC, 6, ':'); + // Serial.println(""); + // missing_mac = false; + // } + + // Serial.println(); //DEBUG +} + + // For Softdevice v6: after received a report, scanner will be paused + // We need to call Scanner resume() to continue scanning + Bluefruit.Scanner.resume(); +} + +int rota = 0; + +void loop() +{ + /* Toggle red LED every second */ + // digitalToggle(LED_RED); +// Telem(10); + + // rota += 10; + // Serial.printf("rotation: %d", rota); + + delay(500); +} From dea49487189e15897f260e7215f17f3e26b92032 Mon Sep 17 00:00:00 2001 From: ItsAlex Date: Sat, 10 Jun 2023 13:46:00 -0700 Subject: [PATCH 16/16] cleanup --- .DS_Store | Bin 8196 -> 8196 bytes niceItsyCode/.DS_Store | Bin 0 -> 6148 bytes niceItsyCode/niceItsyCode.ino | 15 --------------- 3 files changed, 15 deletions(-) create mode 100644 niceItsyCode/.DS_Store diff --git a/.DS_Store b/.DS_Store index 133eef2be8f20a78de244fabd14d37c2537af8f0..c7108d9d1bbb304c58762fede1cf11bf26a93aaa 100644 GIT binary patch delta 40 pcmZp1XmOa}&nUJrU^hRb*k&Gq64uQJMBSMtHaKi%m#BnsYyl2@4LSe- delta 84 zcmZp1XmOa}&nUhzU^hRb_+}n~5>`fm$qPj!CI<+M@iE?JU|-s>NHkZb+!sXR2TMqb)O-LXp+Jgq1yImk$FpHtfH~a}A0DRW`|cCFYK#%-JY$Pjyx<-8=qK6d39s#N!ZY@G8r2`T zUXnkd-yZw@?%3}l7q}xXKf}%+Dpm?e0VyB_q<|E-sDSrg+WaO_Q3^-_De$9!e;*3n zu{InML;gFc+u(+A$w41F> zC>FPqFOd$biHcG{3XB!F&SmHQe?z}8|Bs8bk^)lTUnyX-^}~9_S8Ba=@^ap53;mJ) qZOn~yhG@maXvJK3D_;D{EB?&=+HgpWa`I75)Q^DcB9j7tp}-frryURg literal 0 HcmV?d00001 diff --git a/niceItsyCode/niceItsyCode.ino b/niceItsyCode/niceItsyCode.ino index 1d6e75d..16c8a4f 100644 --- a/niceItsyCode/niceItsyCode.ino +++ b/niceItsyCode/niceItsyCode.ino @@ -108,21 +108,6 @@ typedef struct node_record_s node_record_t records[ARRAY_SIZE]; -String pad4( int input ){ - - if ( input < 10) { - return "000" + String(input); - } - if ( input < 100) { - return "00" + String(input); - } - if ( input < 1000) { - return "0" + String(input); - } - return String(input); - -} - String pad3( int input ){ if ( input < 10) {