From 1a9fd6052497d7c2977b4c1a4a6540c5c8b61264 Mon Sep 17 00:00:00 2001 From: psycodad Date: Sun, 7 Jul 2019 12:54:52 +0200 Subject: [PATCH] initial import from Mathias' work --- README.md | 5 +- witty.ino | 289 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 293 insertions(+), 1 deletion(-) create mode 100644 witty.ino diff --git a/README.md b/README.md index 0adfd86..9ef855b 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,6 @@ # witty -Witty development \ No newline at end of file +Witty development sketch + +## Requirements +Currently needs *MPU6050* Arduino library (installable through library manager) diff --git a/witty.ino b/witty.ino new file mode 100644 index 0000000..23bd6bb --- /dev/null +++ b/witty.ino @@ -0,0 +1,289 @@ +#define DEBUG 1 + +#include "I2Cdev.h" +#include "MPU6050.h" +#include "Wire.h" +#define mRd 7 +#define mRa 6 +#define mLd 4 +#define mLa 5 +#define Gry_offset 0 //The offset of the gyro +#define Gyr_Gain 8.000 // 16.348 + +#define Angle_offset 0 // The offset of the accelerator +float Angle_Delta, Angle_Recursive, Angle_Confidence; + +#define RMotor_offset_fwd 0 // The offset of the Motor +#define LMotor_offset_fwd 0 // The offset of the Motor +#define RMotor_offset_bck 0 // The offset of the Motor +#define LMotor_offset_bck 0 // The offset of the Motor +#define Mramp 3 +#define pi 3.14159 +float kp, ki, kd; +float Angle_Raw, Angle_Filtered, omega; +float Turn_Speed = 0, Turn_Speed_K = 0; +float Run_Speed = 0, Run_Speed_K = 0; +float LOutput, ROutput; +float LlastA, RlastA; +int LlastD, RlastD; +unsigned long preTime = 0; +unsigned long lastTime; +float Input, Output; +float errSum, dErr, error, lastErr; +int timeChange; + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 accelgyro; +MPU6050 initialize; + +int16_t ax, ay, az; +int16_t gx, gy, gz; + + +void setup() { +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); +#endif + Serial.begin(115200); + Serial.println("Initializing I2C devices..."); + accelgyro.initialize(); + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);; + for (int i = 0; i < 100; i++) // Looping 200 times to get the real gesture when starting + { + Filter(); + } + + pinMode(mRd, OUTPUT); + pinMode(mRa, OUTPUT); + pinMode(mLd, OUTPUT); + pinMode(mLa, OUTPUT); + LlastA = 0.0; + RlastA = 0.0; + LlastD = LOW; + RlastD = LOW; +} + +void loop() { + // put your main code here, to run repeatedly: + Filter(); +#ifdef DEBUG +// Serial.print(" Raw =\t"); +// Serial.print(Angle_Raw); + Serial.print(" Angle =\t"); + Serial.print(Angle_Filtered); +#endif + // If angle > 45 or < -45 then stop the robot + if (abs(Angle_Filtered) < 45) + { + // optional position to check external input + myPID(); + PWMControl(); + } + else + { + // stop wheels + digitalWrite(mRd, LOW); + analogWrite(mRa, 0); + digitalWrite(mLd, LOW); + analogWrite(mLa, 0); + for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down + { + Filter(); + } + if (abs(Angle_Filtered) < 45) // Empty datas and restart the robot automaticly + { + for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds + { + omega = Angle_Raw = Angle_Filtered = 0; + Filter(); + Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0; + myPID(); + } + } + } +#ifdef DEBUG + Serial.println(); + delay(DEBUG); +#endif +} + +void Filter() +{ + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + //Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset); + Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset); + omega = -gx / Gyr_Gain + Gry_offset; + // Filters datas to get the real gesture + unsigned long now = micros(); + float dt = (now - preTime) * 0.000001; + preTime = now; + float K = 0.8; + float A = K / (K + dt); + Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64; + Angle_Recursive = Angle_Delta * dt + Angle_Recursive; + Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega; + Angle_Filtered = Angle_Confidence * dt + Angle_Filtered; + // Preparing datas which need to be shown on the LCD. +} + +void myPID() +{ + kp = 1.000; // 22.000 + ki = 0; + kd = 0.00; // 1.60 + // Calculating the output values using the gesture values and the PID values. + error = Angle_Filtered; + errSum += error; + dErr = error - lastErr; + Output = kp * error + ki * errSum + kd * omega; + lastErr = error; +#ifdef DEBUG +// Serial.print("\t error:\t"); + // Serial.print(error); +// Serial.print("\t errSum:\t"); +// Serial.print(errSum); +// Serial.print("\t dErr:\t"); +// Serial.print(dErr); +// Serial.print("\t omega:\t"); +// Serial.print(omega); + Serial.print("\t output:\t"); + Serial.print(Output); +// Output = 0; +#endif + LOutput = Output - Turn_Speed + Run_Speed; + ROutput = Output + Turn_Speed + Run_Speed; +} + +void PWMControl() +{ + // forward min 15 both + // reverse min 35 both + int sL, sR; + + if (LOutput >= 0.0) { + // move forward + sL = min(255, LMotor_offset_fwd + LOutput); + if (LlastD == HIGH){ + fade( mLa, LlastD, LOW, LlastA, 127); + digitalWrite(mLd, LOW); + } + fade( mLa, LlastD, LOW, LlastA, sL); + LlastD = LOW; + LlastA = sL; + } + if (LOutput < 0.0) { + sL = max(0, 255 - LMotor_offset_bck + LOutput); + if (LlastD == LOW){ + fade( mLa, LlastD, HIGH, LlastA, 127); + digitalWrite(mLd, HIGH); + } + fade( mLa, LlastD, HIGH, LlastA, sL); + LlastD = HIGH; + LlastA = sL; + } + if (ROutput >= 0.0) { + sR = min(255, RMotor_offset_fwd + ROutput); + if (RlastD == HIGH){ + fade( mRa, RlastD, LOW, RlastA, 127); + digitalWrite(mRd, LOW); + } + fade( mRa, RlastD, LOW, RlastA, sR); + RlastD = LOW; + RlastA = sR; + } + if (ROutput < 0.0) { + sR = max(0, 255 - RMotor_offset_bck + ROutput); + if (RlastD == LOW){ + fade( mRa, RlastD, HIGH, RlastA, 127); + digitalWrite(mRd, HIGH); + } + fade( mRa, RlastD, HIGH, RlastA, sR); + RlastD = HIGH; + RlastA = sR; + } +} + +void fade(int motor, int oD, int nD, int oA, int nA) { + analogWrite(motor, nA); +} + +void fadeX(int motor, int oD, int nD, int oA, int nA) { + int wait = 30; + if (nA == 0) { + // stop motor + if (oD == HIGH) { + // fade tp A==D==High==255 + for (int i = oA; i <= 255; i++) { + analogWrite(motor, i); + delay(wait); + } + } else { + // fade to A==D==Low==0 + for (int i = oA; i >= 0; i--) { + analogWrite(motor, i); + delay(wait); + } + } + } + if (nA > 0) { + // move forward + if (oD == HIGH) { + // fade to no motion as last driection was reverse + for (int i = oA; i <= 255; i++) { + analogWrite(motor, i); + delay(wait); + } + digitalWrite(motor, LOW); + for (int i = 0; i <= nA; i++) { + analogWrite(motor, i); + delay(wait); + } + } else { + if (oA > nA) { + for (int i = oA; i >= nA; i--) { + analogWrite(motor, i); + delay(wait); + } + } else { + for (int i = oA; i <= nA; i++) { + analogWrite(motor, i); + delay(wait); + } + } + } + } + if (nA < 0) { + // move reverse + if (oD == LOW) { + // fade to no motion as last driection was forward + for (int i = oA; i >= 0; i--) { + analogWrite(motor, i); + delay(wait); + } + digitalWrite(motor, HIGH); + for (int i = 255; i >= nA; i--) { + analogWrite(motor, i); + delay(wait); + } + } else { + if (oA > nA) { + for (int i = oA; i >= nA; i--) { + analogWrite(motor, i); + delay(wait); + } + } else { + for (int i = oA; i <= nA; i++) { + analogWrite(motor, i); + delay(wait); + } + } + } + } +}