Non riesco a capire dove sia l'errore, ho controllato ogni cosa, comprese le librerie e non manca nulla. Nonostante tutto l'errore rimane. Aiutatemi per favore, grazie mille.
Messaggio di errore:
sketch_jan28a.ino: In function 'void setup()':
sketch_jan28a:47: error: 'i2cWrite' was not declared in this scope
sketch_jan28a:48: error: 'i2cWrite' was not declared in this scope
sketch_jan28a:50: error: 'i2cRead' was not declared in this scope
sketch_jan28a:58: error: 'i2cRead' was not declared in this scope
sketch_jan28a.ino: In function 'void dof()':
sketch_jan28a:131: error: 'i2cRead' was not declared in this scope
'i2cWrite' was not declared in this scope
Codice:
#include <Wire.h>
#include "Kalman.h"
Kalman kalmanX;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accXangle;
float gyroXangle;
float kalAngleX;
unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;
const int AIN1 = 3;
const int AIN2 = 9;
const int BIN1 = 10;
const int BIN2 = 11;
int speed;
// PID
const float Kp = 4;
const float Ki = 1;
const float Kd = 1;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
void setup() {
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(57600);
Wire.begin();
i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false));
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100);
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle);
gyroXangle = accXangle;
timer = micros();
}
void loop() {
runEvery(25)
{
dof();
if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)
{
stop();
}
else{
if (CurrentAngle < 230 && CurrentAngle > 130)
{
Pid();
Motors();
}
else
{
stop();
}
}
}
}
void Motors(){
if (speed > 0)
{
//forward
analogWrite(AIN1, speed);
analogWrite(AIN2, 0);
analogWrite(BIN1, speed);
analogWrite(BIN2, 0);
}
else
{
// backward
speed = map(speed,0,-255,0,255);
analogWrite(AIN1, 0);
analogWrite(AIN2, speed);
analogWrite(BIN1, 0);
analogWrite(BIN2, speed);
}
}
void stop()
{
analogWrite(AIN1, 0);
analogWrite(AIN2, 0);
analogWrite(BIN1, 0);
analogWrite(BIN2, 0);
}
void Pid(){
error = 180 - CurrentAngle;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
}
