Hello,
Je suis en train de construire un robot et lorsque j' upload le code la led orange reste fixe.
Savez-vous ce que cela signifie?
Merci
PS, c'est une arduino Uno
Salut,
Il faudrait que l'on sache quel code utilises tu ??, pour savoir ce qui se passe.
@+
Voici les parties de code :
#include <Kalman.h>
// Rutine for repeat part of the code every X miliseconds
#define runEvery(t) for (static long _lasttime;\
(uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
_lasttime += (t))
//Set up a timer Variable
uint32_t timer;
double InputRoll; // Roll angle value
double InitialRoll; // Roll initial angle
void setup() {
Serial.begin(9600);
timer = micros(); // Initialize timer
InitSensors(); // Initialize sensors
InitMotors(); // Initialize motors
delay(1000); // Wait until sensors be ready
InitialValues(); // Get the initial angle values
SetSetpoint(0); // Set the PID setpoint to 0
}
double RollAngle =0; // Roll angle variable
void loop() {
runEvery(10){ // Exetutes this part of the code every 10 miliseconds -> 100Hz
RollAngle = 0.98* (RollAngle + getDGyroRoll()) + 0.02 * (getAccelRoll());
timer = micros(); // Reset the timer
MotorControl(Compute(RollAngle-InitialRoll)); // Sends the real roll angle -> Roll - InitialRoll
}
ReceiveData(); // Checks Serial for incoming data
}
// This function is used to debug the robot, changig the set
// value of the PID
void ReceiveData(){
if (Serial.available()){
char a = Serial.read();
switch (a){
case 'q':
SetSetpoint(GetSetPoint()+0.01);
break;
case 'w':
SetSetpoint(GetSetPoint()-0.01);
break;
case 'e':
SetSetpoint(GetSetPoint()+0.1);
break;
case 'r':
SetSetpoint(GetSetPoint()-0.1);
break;
case 's':
InitialRoll = RollAngle;
Show(RollAngle);
break;
}
}
}
// Show data via serial port
void Show(double a){
Serial.print("DATOS: ");
Serial.print("Initial Roll: ");
Serial.print(InitialRoll);
Serial.print("Roll: ");
Serial.print(a);
Serial.print("Real Roll: ");
Serial.println(RollAngle-InitialRoll);
}
// PArtie 2 :
// Motor Pins
int enablea = 3;
int enableb = 9;
int a1 = 5;
int a2 = 4;
int b1 = 10;
int b2 = 11;
// Prueba set point
double distancia = 0;
void InitMotors(){
// Set pins as outputs
pinMode(enablea, OUTPUT);
pinMode(enableb, OUTPUT);
pinMode(a1, OUTPUT);
pinMode(a2, OUTPUT);
pinMode(b1, OUTPUT);
pinMode(b2, OUTPUT);
// Set direction to none direction
digitalWrite(a1,HIGH);
digitalWrite(a2,HIGH);
digitalWrite(b1,HIGH);
digitalWrite(b2,HIGH);
}
void MotorControl(double out){
// Sets direction
if (out > 0){ // forward
digitalWrite(a1,HIGH);
digitalWrite(a2,LOW);
digitalWrite(b1,LOW);
digitalWrite(b2,HIGH);
}else{ // backward
digitalWrite(a1,LOW);
digitalWrite(a2,HIGH);
digitalWrite(b1,HIGH);
digitalWrite(b2,LOW);
}
byte vel = abs(out); // Absolute value of velocity
// Checks velocity fits the max ouptut range
if (vel<0)
vel=0;
if (vel > 255)
vel=255;
// Writes the PWM
analogWrite(enablea,vel);
analogWrite(enableb,vel);
}
// PArtie 3 :
// PID variables
int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm =0;
// PID constants
// You can change this values to adjust the control
double kp = 100; // Proportional value
double ki = 2; // Integral value
double kd = 0; // Derivative value
double Setpoint = 0; // Initial setpoint is 0
// Calculates the PID output
double Compute(double input)
{
double error = Setpoint - input;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (input - lastInput);
// Compute PID Output
double output = kp * error + ITerm + kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
// Remember some variables for next time
lastInput = input;
return output;
}
// Seters and geters for Setpoint
void SetSetpoint(double d){
Setpoint = d;
}
double GetSetPoint(){
return Setpoint;
}
La dernière partie du code arrive dans 5 minutes.
En attendant voici une photo de la carte :
dernière partie du code :
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>
// Gyro variables
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
int L3G4200D_Address = 105;
// Accel
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);
//Set up accelerometer variables
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
double accRoll = 0;
//Set up gyroscope variables
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroRoll = 0;
//double gyro_sensitivity = 70; //From datasheet, depends on Scale, 2000DPS = 70, 500DPS = 17.5, 250DPS = 8.75.
// Gyro Variables
int x;
int y;
int z;
void InitSensors(){
Wire.begin(); // Initialize I2C
// ¿200?
setupL3G4200D(2000); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(1500);
if(!accel.begin()) // Initialize Accelerometer
{
/* There was a problem detecting the ADXL345 ... check your connections */
Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
while(1);
}
accel.setRange(ADXL345_RANGE_2_G); // Configure accel
sensors_event_t event; // Variable for accel data
// Calculate bias for the Gyro and accel, the values it gives when it's not moving
// You have to keep the robot vertically and static for a few seconds
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision
getGyroValues(); // Get gyro data
gyroBiasX += (int)x;
gyroBiasY += (int)y;
gyroBiasZ += (int)z;
accel.getEvent(&event); // Get accel data
accBiasX += event.acceleration.x;
accBiasY += event.acceleration.y;
accBiasZ += event.acceleration.z;
delay(1);
}
// Final bias values for every axis
gyroBiasX = gyroBiasX / 100;
gyroBiasY = gyroBiasY / 100;
gyroBiasZ = gyroBiasZ / 100;
accBiasX = accBiasX / 100;
accBiasY = accBiasY / 100;
accBiasZ = accBiasZ / 100;
//Get Starting Pitch and Roll
accel.getEvent(&event);
accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;
if (accRoll <= 360 & accRoll >= 180){
accRoll = accRoll - 360;
}
//gyroRoll = accRoll;
}
void InitialValues(){
// Accelerometer
sensors_event_t event;
double InitialAngle = 0;
double dGyro = 0;
for(int i=1; i < 100; i++){ // Takes 100 values to get more precision
accel.getEvent(&event);
accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG;
getGyroValues();
gyroRateX = ((int)x - gyroBiasX)*.07;
dGyro = gyroRateX * ((double)(micros() - timer)/1000000);
InitialAngle = 0.98* (InitialAngle + dGyro) + 0.02 * (accRoll);
timer = micros();
delay(1);
}
InitialRoll = InitialAngle;
Serial.print("Roll Inicial: ");
Serial.println(InitialRoll);
}
// Roll from accelerometer
double getAccelRoll(){
sensors_event_t event;
accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG; // Calculate the value of the angle
if (accRoll <= 360 & accRoll >= 180){
accRoll = accRoll - 360;
}
return accRoll;
}
// Roll from gyroscope
double getGyroRoll(){
getGyroValues(); // Get values from gyro
// read raw angular velocity measurements from device
gyroRateX = -((int)x - gyroBiasX)*.07;
//gyroRateY = -((int)y - gyroBiasY)*.07;
//gyroRateZ = ((int)z - gyroBiasZ)*.07;
gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
return gyroRoll;
}
// Angular velocity of Roll by gyroscope
double getDGyroRoll(){
getGyroValues(); // Get values from gyro
// read raw angular velocity measurements from device
gyroRateX = -((int)x - gyroBiasX)*.07;
//gyroRateY = -((int)y - gyroBiasY)*.07;
//gyroRateZ = ((int)z - gyroBiasZ)*.07;
double dgyroRoll = gyroRateX * ((double)(micros() - timer)/1000000);
return dgyroRoll;
}
/////////////////////////////////////////////////////////////
////////////// Gyro Sensor Code //////////////////
/////////////////////////////////////////////////////////////
// This part of code if from Jim Lindblom of Sparkfun's code
// used for read the data from the gyro sensor: L3G4200D
void getGyroValues(){
byte xMSB = readRegister(L3G4200D_Address, 0x29);
byte xLSB = readRegister(L3G4200D_Address, 0x28);
x = ((xMSB << 8) | xLSB);
byte yMSB = readRegister(L3G4200D_Address, 0x2B);
byte yLSB = readRegister(L3G4200D_Address, 0x2A);
y = ((yMSB << 8) | yLSB);
byte zMSB = readRegister(L3G4200D_Address, 0x2D);
byte zLSB = readRegister(L3G4200D_Address, 0x2C);
z = ((zMSB << 8) | zLSB);
}
int setupL3G4200D(int scale){
//From Jim Lindblom of Sparkfun's code
// Enable x, y, z and turn off power down:
writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
// Configure CTRL_REG3 to generate data ready interrupt on INT2
// No interrupts used on INT1, if you'd like to configure INT1
// or INT2 otherwise, consult the datasheet:
writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
// CTRL_REG4 controls the full-scale range, among other things:
if(scale == 250){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
}else if(scale == 500){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
}else{
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
}
// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you'd like:
writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1); // read a byte
while(!Wire.available()) {
// waiting
}
v = Wire.read();
return v;
}