psycodad
5 years ago
parent
0cc79be53f
commit
1a9fd60524
@ -1,3 +1,6 @@
|
||||
# witty
|
||||
|
||||
Witty development
|
||||
Witty development sketch
|
||||
|
||||
## Requirements
|
||||
Currently needs *MPU6050* Arduino library (installable through library manager)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in new issue