migrate

FlyTri.ino

@@ -0,0 +1,177 @@
+#include "MPU6050.h"
+#include <Servo.h>
+#include "math.h"
+#define CHANNEL_SIZE 6
+#define IBUS_BUFFSIZE 32
+
+// Define all the variables
+int16_t ax, ay, az, gx, gy, gz;
+int throttle, tailServoAngle, frontLeftSpeed, frontRightSpeed, backMotorSpeed;
+
+static uint8_t ibusIndex=0;
+static uint8_t ibus[IBUS_BUFFSIZE]={0}; 
+uint16_t data[CHANNEL_SIZE];
+
+// PID constants
+float kpRoll = 0.8;
+float kiRoll = 0.0;
+float kdRoll = 0.0;
+
+float kpPitch = 0.8;
+float kiPitch = 0.0;
+float kdPitch = 0.0;
+
+// Target angles for balance
+float targetRoll = 0.0;
+float targetPitch = 0.0;
+
+// Variables for PID control
+float prevErrorRoll = 0.0;
+float prevErrorPitch = 0.0;
+float integralRoll = 0.0;
+float integralPitch = 0.0;
+
+// Define all ESCs
+Servo frontLeft;
+Servo frontRight;
+Servo backMiddle;
+
+// Define tail servo
+Servo tailServo;
+
+// Define gyro and accelerometer
+MPU6050 mpu;
+
+void setup() {
+
+  // Set ibus port baudrate
+  Serial.begin(115200);
+
+  // Init MPU
+  mpu.initialize();
+
+  // Set builtin LED
+  pinMode(13, OUTPUT);
+  pinMode(3, OUTPUT);
+  pinMode(9, OUTPUT);
+  pinMode(10, OUTPUT);
+  pinMode(11, OUTPUT);
+  digitalWrite(13,0);
+
+  // Attach servo and ESC to pins
+  tailServo.attach(3);              // Attach tail servo to pin 3
+  frontLeft.attach(9,1000,2000);    // Attach front left motor to 9
+  frontRight.attach(10,1000,2000);  // Attach front left motor to 10
+  backMiddle.attach(11,1000,2000);  // Attach front left motor to 11
+  // Init all 3 ESCs
+  armESC();
+
+  // Turn on LED to signal ready status
+  digitalWrite(13,1);
+}
+
+void loop() {
+
+  // Update gyro and IBUS receiver
+  read_serial();
+  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+  throttle = get_channel(2);
+  tailServoAngle =  get_channel(3);
+  targetRoll = map(get_channel(0), 1000, 2000, 40, -40);
+  targetPitch = map(get_channel(1), 1000, 2000, -40, 40);
+
+  // Calculate angles
+  float roll = atan2(ay, az) * 180.0 / PI;
+  float pitch = atan2(ax, az) * 180.0 / PI;
+
+  // Calculate errors
+  float errorRoll = targetRoll - roll;
+  float errorPitch = targetPitch - pitch;
+
+  // Calculate PID terms
+  float pidRoll = kpRoll * errorRoll + kiRoll * integralRoll + kdRoll * (errorRoll - prevErrorRoll);
+  float pidPitch = kpPitch * errorPitch + kiPitch * integralPitch + kdPitch * (errorPitch - prevErrorPitch);
+
+  // Update integral terms
+  integralRoll += errorRoll;
+  integralPitch += errorPitch;
+
+  // Update previous errors
+  prevErrorRoll = errorRoll;
+  prevErrorPitch = errorPitch;
+
+  // Calculate motor speeds based on PID outputs
+  frontLeftSpeed = constrain(throttle + pidRoll + pidPitch, 1000, 2000);
+  frontRightSpeed = constrain(throttle - pidRoll + pidPitch, 1000, 2000);
+  backMotorSpeed = constrain(throttle - pidRoll - pidPitch, 1000, 2000);
+
+  if (throttle<1010) {
+      frontLeftSpeed=1000;
+      frontRightSpeed=1000;
+      backMotorSpeed=1000;
+  }
+  // Send signals to motors and servo
+  frontLeft.writeMicroseconds(frontLeftSpeed);
+  frontRight.writeMicroseconds(frontRightSpeed);
+  backMiddle.writeMicroseconds(backMotorSpeed);
+  tailServo.writeMicroseconds(tailServoAngle);
+  delay(1);
+}
+
+
+void read_serial(void) {
+  uint8_t i;
+  uint16_t chksum = 0xFFFF; // Initialize chksum to a proper value
+  uint16_t rxsum;
+  while (Serial.available()) {
+    uint8_t val = Serial.read();
+    if (ibusIndex == 0 && val != 0x20) {
+      continue;
+    }
+    if (ibusIndex == 1 && val != 0x40) {
+      ibusIndex = 0;
+      continue;
+    }
+    if (ibusIndex < IBUS_BUFFSIZE) {
+      ibus[ibusIndex] = val;
+    }
+    ibusIndex++;
+    if (ibusIndex == IBUS_BUFFSIZE) {
+      ibusIndex = 0;
+      for (i = 0; i < 30; i++) {
+        chksum -= ibus[i];
+      }
+      rxsum = ibus[30] + (ibus[31] << 8);
+      if (chksum == rxsum) {
+        data[0] = (ibus[3] << 8) + ibus[2];
+        data[1] = (ibus[5] << 8) + ibus[4];
+        data[2] = (ibus[7] << 8) + ibus[6];
+        data[3] = (ibus[9] << 8) + ibus[8];
+        data[4] = (ibus[11] << 8) + ibus[10];
+        data[5] = (ibus[13] << 8) + ibus[12];
+        return;
+      }
+    }
+  }
+}
+
+uint16_t get_channel(uint8_t channel) {
+    return data[channel];
+}
+
+void armESC() {
+  // Arm the ESCs
+  frontLeft.writeMicroseconds(1000); 
+  frontRight.writeMicroseconds(1000);
+  backMiddle.writeMicroseconds(1000);
+  delay(1000);
+  frontLeft.writeMicroseconds(2000); 
+  frontRight.writeMicroseconds(2000);
+  backMiddle.writeMicroseconds(2000);
+  delay(500);
+  frontLeft.writeMicroseconds(1000);
+  frontRight.writeMicroseconds(1000);
+  backMiddle.writeMicroseconds(1000);
+  delay(1000);
+}

Readme.md

@@ -0,0 +1,44 @@
+# Tricopter flight controller
+
+This repository contains lightweight Arduino code for stabilizing and controlling a tricopter using a FlySky Fli 14+ receiver and a MPU6050 accelerometer and gyroscope module.
+
+## Overview
+
+This Arduino sketch is designed to stabilize and control a tricopter using data from a FlySky Fli 14+ receiver for remote control input and a MPU6050 module for measuring the orientation of the tricopter. The sketch calculates PID control values to adjust motor speeds and tail servo angle in order to maintain the desired roll and pitch angles.
+
+## Features
+
+- Tricopter stabilization and control using PID control.
+- Uses FlySky Fli 14+ receiver to receive remote control signals.
+- Utilizes MPU6050 accelerometer and gyroscope for orientation measurement.
+- PID control for roll and pitch stabilization.
+- Arm and initialize ESCs for motor control.
+- Customizable PID constants for roll and pitch control.
+
+## Requirements
+
+- Arduino board compatible with the sketch (tested with Arduino Nano).
+- FlySky Fli 14+ receiver for remote control.
+- MPU6050 accelerometer and gyroscope module for orientation measurement.
+- Servo motors for controlling motor speeds and tail servo.
+
+## Getting Started
+
+1. Connect the FlySky Fli 14+ receiver to the Arduino board on RX pin
+2. Connect the MPU6050 module to the Arduino board.
+3. Connect ESCs for motor control and tail servo (pins 9, 10, 11, and 3 in the provided code).
+4. Adjust the PID constants (kpRoll, kiRoll, kdRoll, kpPitch, kiPitch, kdPitch) to tune the tricopter's stabilization performance.
+5. Upload the provided code to the Arduino board.
+6. Power on the tricopter and make sure it's on a stable surface.
+7. Observe the tricopter's behavior as it attempts to maintain the target roll and pitch angles.
+
+## Notes
+
+- Ensure that the ESCs are properly calibrated and the motors spin in the correct direction.
+- Make sure the tricopter is operated in a safe environment, away from people and obstacles.
+- Tune the PID constants to achieve stable and responsive flight performance.
+
+
+## License
+
+This project is licensed under the [GNU General Public License (GPL)](LICENSE).