ich habe jetzt erstmal ein code ohne PID ausprobiert:
Code:
#include <Arduino.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motor_A = AFMS.getMotor(3);
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);
#define TRIGGER_PIN 6 //9
#define ECHO_PIN 7 //8
#define MAX_DISTANCE 175
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
void setup()
{
// initialize serial.print
Serial.begin(115200);
Serial1.begin(115200);
Serial.println("AFMS test_1 ohne PID");
Serial1.println("AFMS test_1 ohne PID");
// initialize AFMS
AFMS.begin();
Wire.begin();
TWBR = 12; // set 400kHz mode @ 16MHz CPU
setupMPU();
}
void loop()
{
static long Q_Timer = millis();
if ((long)( millis() - Q_Timer ) >= 20)
{
Q_Timer = millis();
recordAccelRegisters();
recordGyroRegisters();
printData();
balance();
}
}
void balance() //120
{
if (gForceX >= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(120);
motor_B->setSpeed(120);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(BACKWARD);
motor_B->run(BACKWARD);
}
}
if (gForceX <= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(120);
motor_B->setSpeed(120);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(FORWARD);
motor_B->run(FORWARD);
}
}
}
void setupMPU()
{
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001
//for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}
void recordAccelRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Accel Registers (3B - 40)
while (Wire.available() < 6);
accelX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
accelY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
processAccelData();
}
void processAccelData()
{
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
}
void recordGyroRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Gyro Registers (43 - 48)
while (Wire.available() < 6);
gyroX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
processGyroData();
}
void processGyroData()
{
rotX = gyroX / 131.0;
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
}
void printData()
{
Serial.print(" accelX= ");
Serial.println(gForceX);
Serial1.print(" accelX= ");
Serial1.println(gForceX);
}
ohne mir zu arg den kopf über die einzelnen blöcke zerbrochen zu haben. Habe einfach angenommen, dass diese funktionieren, habe es an meine hardware angepasst und habe versucht die zusammenhänge ein bischen zu begreifen.
einen adapter mit drei potis habe ich auch schon angefertig:
Aber erstmal habe ich versucht die arduino PID-V1 library mit vorgegebenen Kp, Kd und Ki einzubauen:
Code:
#include <Arduino.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#include <PID_v1.h>
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motor_A = AFMS.getMotor(3);
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);
#define TRIGGER_PIN 6 //9
#define ECHO_PIN 7 //8
#define MAX_DISTANCE 175
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
/************************/
double Setpoint, Input, Output, motorPower, motorPowerSum;
PID myPID(&Input, &Output, &Setpoint, 40, 5, 1, REVERSE);
/***********************/
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
void setup()
{
// initialize serial.print
Serial.begin(115200);
Serial1.begin(115200);
Serial.println("AFMS test_1 mit PID library");
Serial1.println("AFMS test_1 mit PID library");
// initialize AFMS
AFMS.begin();
Wire.begin();
TWBR = 12; // set 400kHz mode @ 16MHz CPU
/*******************/
// initialize PID sampling loop
Setpoint = 0.3;
//myPID.SetSampleTime(100);
myPID.SetOutputLimits(-255, 255);
myPID.SetMode(AUTOMATIC);
/*******************/
setupMPU();
}
void loop()
{
static long Q_Timer = millis();
if ((long)( millis() - Q_Timer ) >= 20)
{
Q_Timer = millis();
recordAccelRegisters();
recordGyroRegisters();
Input = gForceX;
myPID.Compute();
analogWrite(3, Output);
if (Output <=0)
{
motorPower = - Output;
}
else
{
motorPower = Output;
}
printData();
balance();
}
}
void balance() //motorPower
{
if (gForceX >= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(motorPower);
motor_B->setSpeed(motorPower);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(BACKWARD);
motor_B->run(BACKWARD);
}
}
if (gForceX <= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(motorPower);
motor_B->setSpeed(motorPower);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(FORWARD);
motor_B->run(FORWARD);
}
}
}
void setupMPU()
{
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001
//for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}
void recordAccelRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Accel Registers (3B - 40)
while (Wire.available() < 6);
accelX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
accelY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
processAccelData();
}
void processAccelData()
{
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
}
void recordGyroRegisters()
{
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); //Request Gyro Registers (43 - 48)
while (Wire.available() < 6);
gyroX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
processGyroData();
}
void processGyroData()
{
rotX = gyroX / 131.0;
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
}
void printData()
{
Serial.print(" accelX= ");
Serial.print(gForceX);
Serial.print(" Input= ");
Serial.print(Input);
Serial.print(" Output= ");
Serial.print(Output);
Serial.print(" motorPower= ");
Serial.println(motorPower);
Serial.print(" motorPowerSum= ");
Serial.println(motorPowerSum);
Serial1.print(" accelX= ");
Serial1.print(gForceX);
Serial1.print(" Input= ");
Serial1.print(Input);
Serial1.print(" Output= ");
Serial1.print(Output);
Serial1.print(" motorPower= ");
Serial1.print(motorPower);
Serial1.print(" motorPowerSum= ");
Serial1.println(motorPowerSum);
}
das hat auch halbwegs geklappt. Der "zweirädler" bzw. der code reagiert so, wie man es erwartet, will heissen, beim kippen um die achse der räder wird gegengesteuert. Während die ausgabe von "accelX" annähernd konstannt bleibt, erhöht sich der wert von "motorPower" kontinuierlich bis 255. Allerdings quälend langsam. Kippe ich den roboter in entgegengesetzte richtung, wird der wert von "motorPower" erstmal genauso langsam wieder runtergezählt, bevor er anfängt genauso langsam wieder hochzuzählen. Die änderung des kippwinkels von "+" in "-" ist in der tabelle ca. bei output=70 zu sehen.
(Die tabelle mit den ausgegebenen werten war zu lang, deshalb als video hier zu sehen...)
Code:
accelX= -0.45 Input= -0.45 Output= -36.20 motorPower= 36.20 motorPowerSum= 0.00
accelX= -0.40 Input= -0.40 Output= -38.47 motorPower= 38.47 motorPowerSum= 0.00
accelX= -0.48 Input= -0.48 Output= -38.47 motorPower= 38.47 motorPowerSum= 0.00
accelX= -0.50 Input= -0.50 Output= -38.47 motorPower= 38.47 motorPowerSum= 0.00
accelX= -0.42 Input= -0.42 Output= -38.47 motorPower= 38.47 motorPowerSum= 0.00
accelX= -0.41 Input= -0.41 Output= -38.47 motorPower= 38.47 motorPowerSum= 0.00
accelX= -0.47 Input= -0.47 Output= -42.38 motorPower= 42.38 motorPowerSum= 0.00
accelX= -0.44 Input= -0.44 Output= -42.38 motorPower= 42.38 motorPowerSum= 0.00
accelX= -0.49 Input= -0.49 Output= -42.38 motorPower= 42.38 motorPowerSum= 0.00
accelX= -0.50 Input= -0.50 Output= -42.38 motorPower= 42.38 motorPowerSum= 0.00
accelX= -0.43 Input= -0.43 Output= -42.38 motorPower= 42.38 motorPowerSum= 0.00
accelX= -0.49 Input= -0.49 Output= -42.93 motorPower= 42.93 motorPowerSum= 0.00
accelX= -0.39 Input= -0.39 Output= -42.93 motorPower= 42.93 motorPowerSum= 0.00
accelX= -0.43 Input= -0.43 Output= -42.93 motorPower= 42.93 motorPowerSum= 0.00
accelX= -0.44 Input= -0.44 Output= -42.93 motorPower= 42.93 motorPowerSum= 0.00
accelX= -0.45 Input= -0.45 Output= -42.93 motorPower= 42.93 motorPowerSum= 0.00
accelX= -0.50 Input= -0.50 Output= -43.62 motorPower= 43.62 motorPowerSum= 0.00
Meine frage:
wie erreiche ich, dass die umschaltung beim kippwinkel schneller erfolgt? Das muss ja irgendwo bei der umsetzung des "output" wertes zu "motorPower" passieren, nur ich habe keinen plan wie. Ich habe in diesem bereich:
Code:
void balance() //motorPower
{
if (gForceX >= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(motorPower);
motor_B->setSpeed(motorPower);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(BACKWARD);
motor_B->run(BACKWARD);
}
}
if (gForceX <= 0.3)
{
static long M_Timer = millis();
if ((long)( millis() - M_Timer ) >= 10)
{
M_Timer = millis();
motor_A->setSpeed(motorPower);
motor_B->setSpeed(motorPower);
/*
motor_A->run(RELEASE);
motor_B->run(RELEASE);
*/
delay(10);
motor_A->run(FORWARD);
motor_B->run(FORWARD);
}
}
}
EDIT: und hier:
Code:
void loop()
{
static long Q_Timer = millis();
if ((long)( millis() - Q_Timer ) >= 20)
{
Q_Timer = millis();
recordAccelRegisters();
recordGyroRegisters();
Input = gForceX;
myPID.Compute();
analogWrite(3, Output);
if (Output <=0)
{
motorPower = - Output;
}
else
{
motorPower = Output;
}
printData();
balance();
}
}
schon alle möglichen varianten ausprobiert, keine wars...
Zitieren
Lesezeichen