diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..c7108d9 Binary files /dev/null and b/.DS_Store differ 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/.DS_Store b/Mega-edition/.DS_Store new file mode 100644 index 0000000..1e6d027 Binary files /dev/null and b/Mega-edition/.DS_Store differ diff --git a/Mega-edition/Mega_Master/Mega_Master.ino b/Mega-edition/Mega_Master/Mega_Master.ino index c004b9b..3f9e15e 100644 --- a/Mega-edition/Mega_Master/Mega_Master.ino +++ b/Mega-edition/Mega_Master/Mega_Master.ino @@ -1,11 +1,8 @@ -/* 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 5/28/23 */ @@ -19,25 +16,13 @@ */ /* --- Latest Update Log --- - - Key: '-' indicates notes, '*' indicates issues that should be looked at - - --- - - 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 debug garbage. + * + * + * + * + */ // ------- Setup Start ------- @@ -45,30 +30,59 @@ #include #include #include + #include + + // Constants bool debugresponse = true; int vescbaudrate = 9600; // Variables + int spd_count = 0; bool increase_spd = true; bool decrease_spd = false; int telem_instance_count = 0; + + int rssiF = 0; + int rssiL = 0; + int rssiR = 0; + // Pins // Analog - int LeftHandbrake = 14; // A0 - int RightHandbrake = 15; // A1 - int LeftInfared = 16; // A2 - 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 - int UltrasonicTrig = 27; - int UltrasonicEcho = 28; + 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 debugdisable() { + return (!digitalRead(DebugSwitchPin) ); + } + + bool bumped() { + return (!digitalRead(BumperPin) ); + } /* All parenthesis in rx -> tx order - Serial (0, 1) - Serial1 (19, 18) - Serial2 (17, 16) - Serial3 (15, 14) @@ -76,26 +90,33 @@ 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 (52, 53) (Left) LoRa: - Software serial (30,31) + Software serial (50,51) - Only software serial pins have to be specified. All others are set by default by Arduino */ - - SoftwareSerial SoftSerialBLE(22, 23); - SoftwareSerial SoftSerialLoRa(30, 31); + Only software ser ial pins have to be specified. All others are set by default by Arduino */ -// ------- Setup End ------- + SoftwareSerial SoftSerialLoRa(50, 51); + SoftwareSerial SoftSerialBLE(52, 53); + void debugrespond ( String text ) { + if(debugresponse){ Serial.println(text); } + } + + +// ------- Setup End ------- // ------- Motor Start ------- VescUart vescML; VescUart vescMR; + int oldSpeedL = 0; + int oldSpeedR = 0; + float getMotorRPM(VescUart vesc) { if (vesc.getVescValues()) { float motor_rpm = vesc.data.rpm; @@ -112,23 +133,91 @@ } } - void setupMotors(){ - - Serial2.begin(vescbaudrate); - vescML.setSerialPort(&Serial2); - - Serial3.begin(vescbaudrate); - vescMR.setSerialPort(&Serial3); - } 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" ); + }} + + 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(RelayLF, HIGH); + digitalWrite(RelayRF, HIGH); + digitalWrite(RelayLR, LOW); + digitalWrite(RelayRR, LOW); + } + + void retractActuators(){ + digitalWrite(RelayLR, HIGH); + digitalWrite(RelayRR, HIGH); + digitalWrite(RelayLF, LOW); + digitalWrite(RelayRF, LOW); + } + + void brake(int state){ //placehlolder for solenoid brakes on the walker + if(state == 1){ + } + else{ + + } + } + +// ------- Actuator End ------- // ------- Sensor Start ------- @@ -153,7 +242,7 @@ 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 ); } @@ -165,7 +254,7 @@ 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 @@ -173,10 +262,10 @@ distanceIRRight = 13*pow(voltsR, -1); // worked out from datasheet graph if ( debugresponse ) { - Serial.print ( "Infared distances: Left: " ); - Serial.print ( distanceIRLeft ); + Serial.print ( "loopIR - Infared distances: Left: " ); + Serial.print ( voltsL ); Serial.print ( " Right: " ); - Serial.println ( distanceIRRight ); + Serial.println ( voltsR ); } } @@ -191,7 +280,7 @@ 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 ); @@ -201,36 +290,17 @@ // ------- Sensor End ------- - - // ------- Bluetooth Serial Start ------- - - void setupBLE(){ - Serial.begin(9600); - Serial1.begin(9600); - SoftSerialBLE.begin(9600); - } - - const unsigned int maxlength = 7; char telemetry[maxlength]; - //----- - - 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); - } - } + char tlmF[maxlength]; + char tlmL[maxlength]; + char tlmR[maxlength]; + //----- int between(int input, int low, int high){ // self explanatory if ( input > high ) { return high; } @@ -238,146 +308,282 @@ else { return input; } } + /* + 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 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); - } - } - + // getRSSI Left - 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) + + SoftSerialBLE.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) + 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 + + 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???) (a.k.a with known length) - 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.print("GetRSSIL - Value: "); + Serial.println(rssiL); + } - if (debugresponse) { - Serial.print("Command Got!: "); - Serial.println(telemetry); + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + + } } + } - bool didexecute = true; + // getRSSI Right - if (telemetry[0] == '8') { // Is redundant? Or is it useful to have a confirmation that this is correctly-formatted data? + 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 + + 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???) + + // --- + + 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() ) ); - else { - if (debugresponse) { Serial.println("Action: Unrecognized"); } - didexecute = false; - } + if (debugresponse) { + Serial.print("GetRSSIR - Value: "); + Serial.println(rssiR); + } + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + } - - else{ - if (debugresponse) { Serial.println("Purpose: Unrecognized"); } - didexecute = false; - /* ------------------------------------------------------ * - * Purpose byte, default case: Unknown Command - * ------------------------------------------------------ */ - } - } + } + } + // getRSSI Forward -// ------- Bluetooth Serial End ------- + 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 + + 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 ------- + // --- -void setupLoRa(){ - SoftSerialLoRa.begin(9600); - } + 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.print("GetRSSIF - Value: "); + Serial.println(rssiF); + } + + } else {} //invalid + + // --- + + tlm_pos = 0; // Await next command + + } + } + } + +// ------- Bluetooth Serial End ------- -// ------- LoRa End ------- -// ------- Control Start ------- +// ------- IMU 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?) + // Basic demo for accelerometer & gyro readings from Adafruit + // LSM6DSO32 sensor added - // Demonstration Contrller - - void DemoControl(){ - - const int maxspeed = 5; + // 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 - static int L = 0; // Establish speed variables - static int R = 0; + float distance_x; - int targetL = maxspeed; // Start by trying to run motors at full - int targetR = maxspeed; + Adafruit_LSM6DSO32 dso32; + + void IMUloop() { - // Target Shifting - if ( distanceUltrasonic < threshUltrasonic || pressureReadingLeft < threshPressure || pressureReadingRight < threshPressure ) { // Anything to stop both motors - targetL = 0; - targetR = 0; - } + // Get a new normalized sensor event + sensors_event_t accel; + sensors_event_t gyro; + sensors_event_t temp; + dso32.getEvent(&accel, &gyro, &temp); - if ( distanceIRLeft < threshIR ) { - targetL = 0; - } + 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"); - if ( distanceIRRight < threshIR ) { - targetR = 0; + // 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; } - // + + if (debugresponse) { Serial.print(accel_x_tared); } + - 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. + 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); + if (debugresponse) { + Serial.println(" m/s^2 "); + + Serial.print(distance_x); + Serial.println(" m "); + } - setMotorSpeed(L, R); + // // 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(); - if (debugresponse) { - - Serial.print ("Left target speed is "); - Serial.print (targetL); - Serial.print (" , left motor is running at "); - Serial.println (L); - 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); + } - } - } - - void FacilityControl(){ - +// ------- IMU End ------- + + + +// ------- Control Start ------- + + int RSSILavg; + int RSSIRavg; + int RSSIFavg; + + void UpdateData(){ + loopUltrasonic(); + loopIR(); + loopPressure(); + getRSSIF(); + getRSSIL(); + getRSSIR(); + motordebug(); + IMUloop(); + } + + void Debugger(){ + /* const int maxspeed = 6; static int Speed = 0; @@ -385,7 +591,7 @@ void setupLoRa(){ int target = 0; // Target Shifting - if ( distanceUltrasonic > threshUltrasonic ) { // Anything to stop both motors + if ( distanceUltrasonic > Ultrasonic ) { // Anything to stop both motors target = maxspeed; } @@ -396,86 +602,433 @@ void setupLoRa(){ 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); Serial.println (""); - } + + */ } - // --- + void RSSIPlotter(){} // ------- Control End ------- +// ------- Nav Start ------- + + void collisionDetect() { + if (distanceUltrasonic > 80) { + setMotorSpeed(-5,-5); + Serial.println("CollisionDetect - Driving!"); + } else { + setMotorSpeed(0,0); + Serial.println("CollisionDetect - Stopped!"); + } + } + + 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 + abs(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 + abs(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 Start ------- // ------- Nav End ------- +// ------- Setup Function Start ------- + + void setup() { + + + debugresponse = true; + + + pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT + pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT + pinMode(RelayLF, OUTPUT); + pinMode(RelayLR, OUTPUT); + pinMode(RelayRF, OUTPUT); + pinMode(RelayRR, OUTPUT); + pinMode(RelayBF, OUTPUT); + pinMode(RelayBR, OUTPUT); -void setup() { + // pinMode(LeftInfared, INPUT); + // pinMode(RightInfared, INPUT); - pinMode(UltrasonicTrig, OUTPUT); // Sets the trigPin as an OUTPUT - pinMode(UltrasonicEcho, INPUT); // Sets the echoPin as an INPUT + pinMode(SDA, INPUT); + pinMode(SCL, INPUT); - setupMotors(); - setupBLE(); - setupLoRa(); // Just starts connection for now + pinMode(SpeakerPin, OUTPUT); + + pinMode(BumperPin, INPUT_PULLUP); + + pinMode(DebugSwitchPin, INPUT_PULLUP); - Serial.println("Mega Master Start!"); -} + // 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 loop() { + debugrespond("Mega Master Start!"); - // GET SENSOR DATA - loopUltrasonic(); - loopInfrared(); - loopPressure(); - delay(100); - // +/* + 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); + } + } + + 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; + } + + // 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 + } + + // 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; + } + + // 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; + } + + */ + + + debugrespond("Actuator test"); + digitalWrite(RelayLR, HIGH); + delay(1000); + digitalWrite(RelayLR, LOW); + delay(1000); + digitalWrite(RelayLF, HIGH); + delay(1000); + digitalWrite(RelayLF, LOW); + delay(1000); - //=====---===== CONTROLLER =====---===== - FacilityControl(); - - //=====---===== CONTROLLER =====---===== + debugrespond("Setup complete"); + + } +// ------- Setup Function End ------- - // ACQUIRE TELEMETRY - delay(300); +// ------- 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 - telem_instance_count = telem_instance_count + 1; - if(telem_instance_count < 6){ - 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 + void loop() { + + while (breakout == false) { // Run the user navigation + + if(debugresponse){ Serial.println("----------------------");} + UpdateData(); + // Autobrake feature + if (pressureReadingLeft >= 150 || pressureReadingRight >= 150) { brake(1); } else { brake(0); } + + if(debugdisable()) { + breakout = true; + 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: + * + * 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 + * (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 + * Types of information (4): rpm, inpVoltage, ampHours, tachometerAbs + * + */ + + + bool targetreached = false; + 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!"); + + } + + + + else if(reason == 2) { // Debugging + debugrespond("Debug mode!"); + debugrespond(""); + + while(debugdisable() == 1){ + + + 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(cumRSSIL); + Serial.print(","); + Serial.println(cumRSSIR); + + delay(200); } + Serial.println("Get out of debug!"); + } + + + + else{ // What?? + Serial.println("How did we get here?"); + delay(1000); + Serial.println("Short timeout for you."); + delay(5000); } - } else { - telem_instance_count = 0; + + if(debugresponse){ Serial.println("Back to the top!"); } + breakout = false; + reason = 0; + + + Serial.println("----------------------"); + Serial.println(""); Serial.println(""); } - -} + +// ------- Loop/Main End ------- diff --git a/niceItsyCode/.DS_Store b/niceItsyCode/.DS_Store new file mode 100644 index 0000000..39b6225 Binary files /dev/null and b/niceItsyCode/.DS_Store differ diff --git a/niceItsyCode/niceItsyCode.ino b/niceItsyCode/niceItsyCode.ino new file mode 100644 index 0000000..16c8a4f --- /dev/null +++ b/niceItsyCode/niceItsyCode.ino @@ -0,0 +1,302 @@ +#include +#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 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); +}