diff --git a/src/controller/drone controller/src/main.cpp b/src/controller/drone controller/src/main.cpp index 3f016dc..669187b 100644 --- a/src/controller/drone controller/src/main.cpp +++ b/src/controller/drone controller/src/main.cpp @@ -5,22 +5,23 @@ // declarations int normalizePot(int pin, int minValue); int mapPot(int normalizedValue); +int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin); -// constants -const int POTPIN1 = 2; -const int MAXPWMVALUE = 1400; +const int MAXPWMVALUE = 1000; const int MINPWMVALUE = 2000; const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8E, 0x5C}; -//Define the struct that will be sent -typedef struct struct_message { + +// Define the struct that will be sent +typedef struct struct_message +{ int PWMCH1; int PWMCH2; int PWMCH3; int PWMCH4; } struct_message; -struct_message myData; //declare the struct as myData +struct_message myData; // declare the struct as myData -esp_now_peer_info_t peerInfo; //create a class object of the ESPNow class +esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class void setup() { @@ -28,52 +29,64 @@ void setup() WiFi.mode(WIFI_STA); // Init ESP-NOW - if (esp_now_init() != ESP_OK) { + if (esp_now_init() != ESP_OK) + { Serial.println("Error initializing ESP-NOW"); return; } // Register peer memcpy(peerInfo.peer_addr, broadcastAddress, 6); - peerInfo.channel = 0; + peerInfo.channel = 0; peerInfo.encrypt = false; - - // Add peer - if (esp_now_add_peer(&peerInfo) != ESP_OK){ + + // Add peer + if (esp_now_add_peer(&peerInfo) != ESP_OK) + { Serial.println("Failed to add peer"); return; } Serial.begin(9600); - pinMode(POTPIN1, INPUT); + + pinMode(D3, OUTPUT); + pinMode(D6, OUTPUT); + pinMode(D7, OUTPUT); + pinMode(D8, OUTPUT); + pinMode(D9, OUTPUT); + pinMode(D0, INPUT); + } void loop() { - Serial.println(mapPot(normalizePot(POTPIN1, 80))); //call normalizePot and put the output into mapPot then print it + Serial.println(analogReadMultiPlexer(0, 0, 0, 0, A0)); // call normalizePot and put the output into mapPot then print it // Set values to send - myData.PWMCH1 = mapPot(normalizePot(POTPIN1, 80)); - myData.PWMCH2 = 1000; //test values + myData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); + myData.PWMCH2 = 1000; // test values myData.PWMCH3 = 1000; myData.PWMCH4 = 1000; - + // Send message via ESP-NOW - esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData)); - - if (result == ESP_OK) { - Serial.println("Sent with success"); + esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData)); + if (result == ESP_OK) + { + // Serial.println("Sent with success"); } - else { - Serial.println("Error sending the data"); + else + { + // Serial.println("Error sending the data"); } + delay(10); } -int mapPot(int normalizedValue){ - return map(normalizedValue, 80, 4095, MINPWMVALUE, MAXPWMVALUE); //map the normalized value to the PWM range +int mapPot(int normalizedValue) +{ + return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range } -int normalizePot(int pin, int minValue) //normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate +int normalizePot(int pin, int minValue) // normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate { int pot = analogRead(pin); @@ -85,4 +98,14 @@ int normalizePot(int pin, int minValue) //normalize the pot value to a range of { return pot; } +} + +int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin) +{ + digitalWrite(D3, LOW); + digitalWrite(D6, LOW); + digitalWrite(D7, LOW); + digitalWrite(D9, LOW); + digitalWrite(D8, LOW); + return analogRead(pin); } \ No newline at end of file diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index ac9ea7b..7e3aa50 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -367,6 +367,7 @@ float invSqrt(float x); void controlMixer(); void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len); void ESPNowSetup(); +void printDebugInfo(); //========================================================================================================================// // VOID SETUP // @@ -476,14 +477,15 @@ void loop() // Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: // printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) // printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) - // printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) - // printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) + printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) + printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) // printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) - // printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) + printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) // printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) // printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) + // printDebugInfo(); //PWM 1 and 5 + Armed condition // Get arming status armedStatus(); // Check if the throttle cut is off and throttle is low. @@ -886,16 +888,7 @@ void calculate_IMU_error() void calibrateAttitude() { // DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators - // Assuming vehicle is powered up on level surface! - /* - * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds - * to boot. - */ - // Warm up IMU and madgwick filter in simulated main loop - for (int i = 0; i <= 10000; i++) - { - prev_time = current_time; - current_time = micros(); + // Assuming vehicle is powered up on level surface!throttle dt = (current_time - prev_time) / 1000000.0; getIMUdata(); Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); @@ -1434,7 +1427,7 @@ void getCommands() channel_2_pwm = myData.PWMCH2; channel_3_pwm = myData.PWMCH3; channel_4_pwm = myData.PWMCH4; - // channel_5_pwm = getRadioPWM(5); //commented out because ESPnow only sends 4 channels + channel_5_pwm = 1000; // Temporary always armed // channel_6_pwm = getRadioPWM(6); #endif @@ -2005,6 +1998,24 @@ void printLoopRate() } } +void printDebugInfo() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print("CH1(thro):"); + Serial.print(channel_1_pwm); + Serial.print(" CH5:"); + Serial.print(channel_5_pwm); + Serial.print(" Armed:"); + Serial.print(armedFly); + Serial.print(" thro_des:"); + Serial.print(thro_des); + Serial.print(" m1_scaled:"); + Serial.print(m1_command_scaled); + Serial.print(" m1_PWM:"); + Serial.println(m1_command_PWM); + } +} + //=========================================================================================// // HELPER FUNCTIONS