boxtec
/
witty
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Compare commits

base: boxtec:1a9fd6052497d7c2977b4c1a4a6540c5c8b61264
Branches Tags
boxtec:Reto
boxtec:master
...
compare: boxtec:d787b8272b5c52c0a1c4a0ffde4bf6a0620d57b7
Branches Tags
boxtec:Reto
boxtec:master

3 Commits
1a9fd60524 ... d787b8272b

Author SHA1 Message Date
Reto d787b8272b Parameter Balance
6 years ago
Reto 3df8d03f3c Parameter Balance
6 years ago
Reto 432372ed1f Parameter Balance
6 years ago
1 changed files with 289 additions and 289 deletions
Show Stats

578
witty.ino
Unescape View File

@ -1,289 +1,289 @@
#define DEBUG 1 #define DEBUG 1
#include "I2Cdev.h" #include "I2Cdev.h"
#include "MPU6050.h" #include "MPU6050.h"
#include "Wire.h" #include "Wire.h"
#define mRd 7 #define mRd 7
#define mRa 6 #define mRa 6
#define mLd 4 #define mLd 4
#define mLa 5 #define mLa 5
#define Gry_offset 0 //The offset of the gyro #define Gry_offset 0 //The offset of the gyro
#define Gyr_Gain 8.000 // 16.348 #define Gyr_Gain 8.000 // 16.348
#define Angle_offset 0 // The offset of the accelerator #define Angle_offset 0 // The offset of the accelerator
float Angle_Delta, Angle_Recursive, Angle_Confidence; float Angle_Delta, Angle_Recursive, Angle_Confidence;
#define RMotor_offset_fwd 0 // The offset of the Motor #define RMotor_offset_fwd 0 // The offset of the Motor
#define LMotor_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 RMotor_offset_bck 0 // The offset of the Motor
#define LMotor_offset_bck 0 // The offset of the Motor #define LMotor_offset_bck 0 // The offset of the Motor
#define Mramp 3 #define Mramp 3
#define pi 3.14159 #define pi 3.14159
float kp, ki, kd; float kp, ki, kd;
float Angle_Raw, Angle_Filtered, omega; float Angle_Raw, Angle_Filtered, omega;
float Turn_Speed = 0, Turn_Speed_K = 0; float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0; float Run_Speed = 0, Run_Speed_K = 0;
float LOutput, ROutput; float LOutput, ROutput;
float LlastA, RlastA; float LlastA, RlastA;
int LlastD, RlastD; int LlastD, RlastD;
unsigned long preTime = 0; unsigned long preTime = 0;
unsigned long lastTime; unsigned long lastTime;
float Input, Output; float Input, Output;
float errSum, dErr, error, lastErr; float errSum, dErr, error, lastErr;
int timeChange; int timeChange;
// class default I2C address is 0x68 // class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here // specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board) // AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69 // AD0 high = 0x69
MPU6050 accelgyro; MPU6050 accelgyro;
MPU6050 initialize; MPU6050 initialize;
int16_t ax, ay, az; int16_t ax, ay, az;
int16_t gx, gy, gz; int16_t gx, gy, gz;
void setup() { void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin(); Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true); Fastwire::setup(400, true);
#endif #endif
Serial.begin(115200); Serial.begin(115200);
Serial.println("Initializing I2C devices..."); Serial.println("Initializing I2C devices...");
accelgyro.initialize(); accelgyro.initialize();
Serial.println("Testing device connections..."); Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);; initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;
for (int i = 0; i < 100; i++) // Looping 200 times to get the real gesture when starting for (int i = 0; i < 100; i++) // Looping 200 times to get the real gesture when starting
{ {
Filter(); Filter();
} }
pinMode(mRd, OUTPUT); pinMode(mRd, OUTPUT);
pinMode(mRa, OUTPUT); pinMode(mRa, OUTPUT);
pinMode(mLd, OUTPUT); pinMode(mLd, OUTPUT);
pinMode(mLa, OUTPUT); pinMode(mLa, OUTPUT);
LlastA = 0.0; LlastA = 0.0;
RlastA = 0.0; RlastA = 0.0;
LlastD = LOW; LlastD = LOW;
RlastD = LOW; RlastD = LOW;
} }
void loop() { void loop() {
// put your main code here, to run repeatedly: // put your main code here, to run repeatedly:
Filter(); Filter();
#ifdef DEBUG #ifdef DEBUG
// Serial.print(" Raw =\t"); // Serial.print(" Raw =\t");
// Serial.print(Angle_Raw); // Serial.print(Angle_Raw);
Serial.print(" Angle =\t"); Serial.print(" Angle =\t");
Serial.print(Angle_Filtered); Serial.print(Angle_Filtered);
#endif #endif
// If angle > 45 or < -45 then stop the robot // If angle > 45 or < -45 then stop the robot
if (abs(Angle_Filtered) < 45) if (abs(Angle_Filtered) < 45)
{ {
// optional position to check external input // optional position to check external input
myPID(); myPID();
PWMControl(); PWMControl();
} }
else else
{ {
// stop wheels // stop wheels
digitalWrite(mRd, LOW); digitalWrite(mRd, LOW);
analogWrite(mRa, 0); analogWrite(mRa, 0);
digitalWrite(mLd, LOW); digitalWrite(mLd, LOW);
analogWrite(mLa, 0); analogWrite(mLa, 0);
for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down
{ {
Filter(); Filter();
} }
if (abs(Angle_Filtered) < 45) // Empty datas and restart the robot automaticly 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 for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds
{ {
omega = Angle_Raw = Angle_Filtered = 0; omega = Angle_Raw = Angle_Filtered = 0;
Filter(); Filter();
Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0; Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
myPID(); myPID();
} }
} }
} }
#ifdef DEBUG #ifdef DEBUG
Serial.println(); Serial.println();
delay(DEBUG); delay(DEBUG);
#endif #endif
} }
void Filter() void Filter()
{ {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset); //Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);
Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset); Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset);
omega = -gx / Gyr_Gain + Gry_offset; omega = -gx / Gyr_Gain + Gry_offset;
// Filters datas to get the real gesture // Filters datas to get the real gesture
unsigned long now = micros(); unsigned long now = micros();
float dt = (now - preTime) * 0.000001; float dt = (now - preTime) * 0.000001;
preTime = now; preTime = now;
float K = 0.8; float K = 0.8;
float A = K / (K + dt); float A = K / (K + dt);
Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64; Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64;
Angle_Recursive = Angle_Delta * dt + Angle_Recursive; Angle_Recursive = Angle_Delta * dt + Angle_Recursive;
Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega; Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega;
Angle_Filtered = Angle_Confidence * dt + Angle_Filtered; Angle_Filtered = Angle_Confidence * dt + Angle_Filtered;
// Preparing datas which need to be shown on the LCD. // Preparing datas which need to be shown on the LCD.
} }
void myPID() void myPID()
{ {
kp = 1.000; // 22.000 kp = 4.110; // 22.000
ki = 0; ki = 0;
kd = 0.00; // 1.60 kd = 0.00; // 1.60
// Calculating the output values using the gesture values and the PID values. // Calculating the output values using the gesture values and the PID values.
error = Angle_Filtered; error = Angle_Filtered;
errSum += error; errSum += error;
dErr = error - lastErr; dErr = error - lastErr;
Output = kp * error + ki * errSum + kd * omega; Output = kp * error + ki * errSum + kd * omega;
lastErr = error; lastErr = error;
#ifdef DEBUG #ifdef DEBUG
// Serial.print("\t error:\t"); // Serial.print("\t error:\t");
// Serial.print(error); // Serial.print(error);
// Serial.print("\t errSum:\t"); // Serial.print("\t errSum:\t");
// Serial.print(errSum); // Serial.print(errSum);
// Serial.print("\t dErr:\t"); // Serial.print("\t dErr:\t");
// Serial.print(dErr); // Serial.print(dErr);
// Serial.print("\t omega:\t"); // Serial.print("\t omega:\t");
// Serial.print(omega); // Serial.print(omega);
Serial.print("\t output:\t"); Serial.print("\t output:\t");
Serial.print(Output); Serial.print(Output);
// Output = 0; // Output = 0;
#endif #endif
LOutput = Output - Turn_Speed + Run_Speed; LOutput = Output - Turn_Speed + Run_Speed;
ROutput = Output + Turn_Speed + Run_Speed; ROutput = Output + Turn_Speed + Run_Speed;
} }
void PWMControl() void PWMControl()
{ {
// forward min 15 both // forward min 15 both
// reverse min 35 both // reverse min 35 both
int sL, sR; int sL, sR;
if (LOutput >= 0.0) { if (LOutput >= 0.0) {
// move forward // move forward
sL = min(255, LMotor_offset_fwd + LOutput); sL = min(255, LMotor_offset_fwd + LOutput);
if (LlastD == HIGH){ if (LlastD == HIGH){
fade( mLa, LlastD, LOW, LlastA, 127); fade( mLa, LlastD, LOW, LlastA, 127);
digitalWrite(mLd, LOW); digitalWrite(mLd, LOW);
} }
fade( mLa, LlastD, LOW, LlastA, sL); fade( mLa, LlastD, LOW, LlastA, sL);
LlastD = LOW; LlastD = LOW;
LlastA = sL; LlastA = sL;
} }
if (LOutput < 0.0) { if (LOutput < 0.0) {
sL = max(0, 255 - LMotor_offset_bck + LOutput); sL = max(0, 255 - LMotor_offset_bck + LOutput);
if (LlastD == LOW){ if (LlastD == LOW){
fade( mLa, LlastD, HIGH, LlastA, 127); fade( mLa, LlastD, HIGH, LlastA, 127);
digitalWrite(mLd, HIGH); digitalWrite(mLd, HIGH);
} }
fade( mLa, LlastD, HIGH, LlastA, sL); fade( mLa, LlastD, HIGH, LlastA, sL);
LlastD = HIGH; LlastD = HIGH;
LlastA = sL; LlastA = sL;
} }
if (ROutput >= 0.0) { if (ROutput >= 0.0) {
sR = min(255, RMotor_offset_fwd + ROutput); sR = min(255, RMotor_offset_fwd + ROutput);
if (RlastD == HIGH){ if (RlastD == HIGH){
fade( mRa, RlastD, LOW, RlastA, 127); fade( mRa, RlastD, LOW, RlastA, 127);
digitalWrite(mRd, LOW); digitalWrite(mRd, LOW);
} }
fade( mRa, RlastD, LOW, RlastA, sR); fade( mRa, RlastD, LOW, RlastA, sR);
RlastD = LOW; RlastD = LOW;
RlastA = sR; RlastA = sR;
} }
if (ROutput < 0.0) { if (ROutput < 0.0) {
sR = max(0, 255 - RMotor_offset_bck + ROutput); sR = max(0, 255 - RMotor_offset_bck + ROutput);
if (RlastD == LOW){ if (RlastD == LOW){
fade( mRa, RlastD, HIGH, RlastA, 127); fade( mRa, RlastD, HIGH, RlastA, 127);
digitalWrite(mRd, HIGH); digitalWrite(mRd, HIGH);
} }
fade( mRa, RlastD, HIGH, RlastA, sR); fade( mRa, RlastD, HIGH, RlastA, sR);
RlastD = HIGH; RlastD = HIGH;
RlastA = sR; RlastA = sR;
} }
} }
void fade(int motor, int oD, int nD, int oA, int nA) { void fade(int motor, int oD, int nD, int oA, int nA) {
analogWrite(motor, nA); analogWrite(motor, nA);
} }
void fadeX(int motor, int oD, int nD, int oA, int nA) { void fadeX(int motor, int oD, int nD, int oA, int nA) {
int wait = 30; int wait = 30;
if (nA == 0) { if (nA == 0) {
// stop motor // stop motor
if (oD == HIGH) { if (oD == HIGH) {
// fade tp A==D==High==255 // fade tp A==D==High==255
for (int i = oA; i <= 255; i++) { for (int i = oA; i <= 255; i++) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} else { } else {
// fade to A==D==Low==0 // fade to A==D==Low==0
for (int i = oA; i >= 0; i--) { for (int i = oA; i >= 0; i--) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} }
} }
if (nA > 0) { if (nA > 0) {
// move forward // move forward
if (oD == HIGH) { if (oD == HIGH) {
// fade to no motion as last driection was reverse // fade to no motion as last driection was reverse
for (int i = oA; i <= 255; i++) { for (int i = oA; i <= 255; i++) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
digitalWrite(motor, LOW); digitalWrite(motor, LOW);
for (int i = 0; i <= nA; i++) { for (int i = 0; i <= nA; i++) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} else { } else {
if (oA > nA) { if (oA > nA) {
for (int i = oA; i >= nA; i--) { for (int i = oA; i >= nA; i--) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} else { } else {
for (int i = oA; i <= nA; i++) { for (int i = oA; i <= nA; i++) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} }
} }
} }
if (nA < 0) { if (nA < 0) {
// move reverse // move reverse
if (oD == LOW) { if (oD == LOW) {
// fade to no motion as last driection was forward // fade to no motion as last driection was forward
for (int i = oA; i >= 0; i--) { for (int i = oA; i >= 0; i--) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
digitalWrite(motor, HIGH); digitalWrite(motor, HIGH);
for (int i = 255; i >= nA; i--) { for (int i = 255; i >= nA; i--) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} else { } else {
if (oA > nA) { if (oA > nA) {
for (int i = oA; i >= nA; i--) { for (int i = oA; i >= nA; i--) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} else { } else {
for (int i = oA; i <= nA; i++) { for (int i = oA; i <= nA; i++) {
analogWrite(motor, i); analogWrite(motor, i);
delay(wait); delay(wait);
} }
} }
} }
} }
} }

Write Preview
Loading…
Cancel
Save