Mon contenu
Non spécifié
Merci pour ta réponse. Actuellement je fais des essais avec le mpu6050 et la mega. J’ai branché deux petits servos pour voir si cela marche. Il me semble que j’ai un problème pour intégrer la commande de direction qui se fait par un potentiomètre. Le but est soit de faire accélérer ou de ralentir une roue pour effectuer le virage. Il y a aussi un problème de sensibilité du mpu. L'amplitude du mouvement de celui-ci me semble trop grande pour actionner les servos?
lorsque j'aurai terminé les essais je remplacerai les servos par le sabertooth.
voici le code que j'utilise actuellement.
#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <TimedAction.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// Steering pot
int steeringPin = A8; // select the input pin for the potentiometer
uint16_t steeringValue;
uint16_t offsetLeft;
uint16_t offsetRight;
//
//Servo motor;
Servo left_Drive;
Servo right_Drive;
//
MPU6050 mpu(0x69);
//
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
//
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//
float gyroPos_Roll = 0;
float gyroPos_Pitch = 0;
//
int DrivePos_Left = 1500;
int DrivePos_Right = 1750;
//
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
boolean blinkState = false;
//
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady(){
mpuInterrupt = true;
}
//
TimedAction steering_Task = TimedAction(100, steering_Update);
//
void setup() {
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
//
left_Drive.attach(10, 800, 2200); // pos.min. max servo(roll)
right_Drive.attach(11, 1500, 1800); // pos. min. max servo(picht)
//
left_Drive.writeMicroseconds(DrivePos_Left);
right_Drive.writeMicroseconds(DrivePos_Right);
//
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
//
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
//
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
//
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
//
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
//
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0 , dmpDataReady, CHANGE);
mpuIntStatus = mpu.getIntStatus();
//
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
//
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
//
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
//
void loop(){
//
// horizon();
// if programming failed, don't try to do anything
if (!dmpReady) return;
//
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
}
//
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
//
// get current FIFO count
fifoCount = mpu.getFIFOCount();
//
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
}else if (mpuIntStatus & 0x02){
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
//
steering_Task.check();
//
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//
gyroPos_Pitch = (ypr[1] * 25); // y (Pitch)
//
DrivePos_Left = DrivePos_Left + gyroPos_Pitch + offsetLeft;
if(DrivePos_Left < 800) DrivePos_Left = 800;
else if (DrivePos_Left > 2200) DrivePos_Left = 2200;
left_Drive.writeMicroseconds(DrivePos_Left);
Serial.print("Drive left : ");Serial.println(DrivePos_Left);
Serial.println("");
Serial.print("Steering left : ");Serial.println(offsetLeft);
Serial.println("______________________");
Serial.println("");
//
DrivePos_Right = DrivePos_Right + gyroPos_Pitch + offsetRight;
if(DrivePos_Right < 1500) DrivePos_Right = 1500;
else if (DrivePos_Right > 2000) DrivePos_Right = 2000;
right_Drive.writeMicroseconds(DrivePos_Right);
Serial.print("Drive right : ");Serial.println(DrivePos_Right);
Serial.println("");
Serial.print("Steering right : ");Serial.println(offsetRight);
Serial.println("______________________");
Serial.println("");
//
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
//
void steering_Update(){
steeringValue = analogRead(steeringPin); // 0 - 1023
//
if (steeringValue < 512){
offsetRight = 0;
offsetLeft = map(steeringValue, 0, 511, 250, 0);
}else if (steeringValue >= 512 || steeringValue <= 1023){
steeringValue = steeringValue - 512;
offsetLeft = 0;
offsetRight = map(steeringValue, 0, 512, 0, 250);
}
}
