You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
423 lines
10 KiB
423 lines
10 KiB
//-----------------------------------------------------------------------//
|
|
// Freaky Witty
|
|
//
|
|
// Serial Commands:
|
|
//
|
|
// a) Change Parameter:
|
|
//
|
|
// kp
|
|
// ki
|
|
// kd
|
|
//
|
|
// molb Motor Offset Left Backwards
|
|
// molf Motor Offset Left Forward
|
|
// morb Motor Offset Right Forward
|
|
// morf Motor Offset Right Backwards
|
|
//
|
|
// Example: kp=4.280&ki=0&kd=0.00&molb=58&morb=58
|
|
//
|
|
// b) Print Parameters:
|
|
//
|
|
// Example: param
|
|
//
|
|
//
|
|
// Tuning:
|
|
// 1. Set Ki and Kd to zero and gradually increase Kp so that the robot starts to oscillate about the zero position.
|
|
//
|
|
// 2. Increase Ki so that the response of the robot is faster when it is out of balance. Ki should be large enough so that the angle of inclination does not increase. The robot should come back to zero position if it is inclined.
|
|
//
|
|
// 3. Increase Kd so as to reduce the oscillations. The overshoots should also be reduced by now.
|
|
//
|
|
//-----------------------------------------------------------------------//
|
|
|
|
//#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
|
|
#define Angle_to_balance 80 // The offset to the balance angle
|
|
float Angle_Delta, Angle_Recursive, Angle_Confidence;
|
|
|
|
int RMotor_offset_fwd = 0; // The offset of the Motor
|
|
int LMotor_offset_fwd = 0; // The offset of the Motor
|
|
int RMotor_offset_bck = 40; //56; // The offset of the Motor
|
|
int LMotor_offset_bck = 40; //56; // 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;
|
|
|
|
// Serial Receive Parameters
|
|
const byte numChars = 32;
|
|
char receivedChars[numChars]; // an array to store the received data
|
|
boolean newData = false;
|
|
|
|
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);;
|
|
|
|
// Set Default Balancing Parameters
|
|
kp = 8.380; // 22.000
|
|
ki = 0.5;
|
|
kd = 0.00; // 1.60
|
|
|
|
for (int i = 0; i < 100; i++) // Looping 100 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() {
|
|
|
|
// check for serial value changes
|
|
recvWithEndMarker();
|
|
if (newData == true) {
|
|
processNewParameters();
|
|
}
|
|
|
|
// 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 > 60 or < -60 then stop the robot
|
|
if (abs(Angle_Filtered - Angle_to_balance ) < 60)
|
|
{
|
|
// 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) < 60) // Empty datas and restart the robot automaticly
|
|
{
|
|
for (int i = 0; i <= 100; 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();
|
|
#endif
|
|
delay(2);
|
|
}
|
|
|
|
void Filter()
|
|
{
|
|
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
|
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;
|
|
}
|
|
|
|
void myPID()
|
|
{
|
|
// Calculating the output values using the gesture values and the PID values.
|
|
error = Angle_Filtered - Angle_to_balance; // move 25°
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void processNewParameters() {
|
|
|
|
Serial.print("New Received Parameters: ");
|
|
Serial.println(receivedChars);
|
|
Serial.println("Process Parameters: ");
|
|
newData = false;
|
|
|
|
char* command = strtok(receivedChars, "&");
|
|
while (command != 0)
|
|
{
|
|
// Split the parameters in two key & values
|
|
char* separator = strchr(command, '=');
|
|
if (separator != 0)
|
|
{
|
|
// Actually split the string in 2: replace '=' with 0
|
|
*separator = 0;
|
|
String parameter = command;
|
|
++separator;
|
|
String separator_s = separator;
|
|
float par_value = separator_s.toFloat();
|
|
Serial.print(" key: ");
|
|
Serial.print(parameter);
|
|
Serial.print(" Val: ");
|
|
Serial.println(par_value);
|
|
|
|
if ( parameter == "kp" ) kp = par_value;
|
|
if ( parameter == "ki" ) ki = par_value;
|
|
if ( parameter == "kd" ) kd = par_value;
|
|
if ( parameter == "molb" ) LMotor_offset_bck = par_value;
|
|
if ( parameter == "molf" ) LMotor_offset_fwd = par_value;
|
|
if ( parameter == "morb" ) RMotor_offset_bck = par_value;
|
|
if ( parameter == "morf" ) RMotor_offset_fwd = par_value;
|
|
|
|
|
|
}
|
|
// Find the next command in input string
|
|
command = strtok(0, "&");
|
|
}
|
|
|
|
// Print all actual Parameters
|
|
|
|
Serial.println("Actual Parameters: ");
|
|
Serial.print("kp: ");
|
|
Serial.print(kp);
|
|
Serial.print(" ki: ");
|
|
Serial.print(ki);
|
|
Serial.print(" kd: ");
|
|
Serial.print(kd);
|
|
|
|
Serial.print(" molb: ");
|
|
Serial.print(LMotor_offset_bck);
|
|
Serial.print(" molf: ");
|
|
Serial.print(LMotor_offset_fwd);
|
|
Serial.print(" morb: ");
|
|
Serial.print(RMotor_offset_bck);
|
|
Serial.print(" morf: ");
|
|
Serial.print(RMotor_offset_fwd);
|
|
|
|
Serial.print(" ");
|
|
|
|
}
|
|
|
|
void recvWithEndMarker() {
|
|
static byte ndx = 0;
|
|
char endMarker = '\n';
|
|
char rc;
|
|
|
|
// if (Serial.available() > 0) {
|
|
while (Serial.available() > 0 && newData == false) {
|
|
rc = Serial.read();
|
|
|
|
if (rc != endMarker) {
|
|
receivedChars[ndx] = rc;
|
|
ndx++;
|
|
if (ndx >= numChars) {
|
|
ndx = numChars - 1;
|
|
}
|
|
}
|
|
else {
|
|
receivedChars[ndx] = '\0'; // terminate the string
|
|
ndx = 0;
|
|
newData = true;
|
|
}
|
|
}
|
|
}
|