Topic: Балансирующий робот
хотел собрать балансирующего робота и привязать его управлению через блютуз, при проверке кода выдает ошибку, подскажите как это исправить?
ошибка при компиляции и код программы ниже
\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
C:\Users\Oleg_Y500\Documents\Arduino\sketch_feb08c\sketch_feb08c.ino: In function 'void setup()':
sketch_feb08c:51: error: 'i2cWrite' was not declared in this scope
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
^
sketch_feb08c:52: error: 'i2cWrite' was not declared in this scope
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
^
sketch_feb08c:54: error: 'i2cRead' was not declared in this scope
while (i2cRead(0x75, i2cData, 1));
^
sketch_feb08c:65: error: 'i2cRead' was not declared in this scope
while (i2cRead(0x3B, i2cData, 6));
^
C:\Users\Oleg_Y500\Documents\Arduino\sketch_feb08c\sketch_feb08c.ino: In function 'void loop()':
sketch_feb08c:91: error: 'i2cRead' was not declared in this scope
while (i2cRead(0x3B, i2cData, 14));
^
exit status 1
'i2cWrite' was not declared in this scope
\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
Пример кода
#include <Wire.h>
#include "Kalman.h"
#define RESTRICT_PITCH
Kalman kalmanX;
Kalman kalmanY;
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Gyroscope angle
double compAngleX, compAngleY; // Complementary filter angle
double kalAngleX, kalAngleY; // Angle after Kalman filter
double corrected_x, corrected_y; // Corrected with offset
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
char a;
double m = 0.7;
double m1 = -0.7;
int d = 0;
int c = 0;
char p;
int in1_motor_left = 8;
int in2_motor_left = 7;
int in3_motor_right = 3;
int in4_motor_right = 4;
int pwm_on = 5; // ms ON
int pwm_off = 5; // ms OFF
//------------------------------------------------------------------------------
void setup() {
// Define outputs
pinMode(in1_motor_left, OUTPUT);
pinMode(in2_motor_left, OUTPUT);
pinMode(in3_motor_right, OUTPUT);
pinMode(in4_motor_right, OUTPUT);
// Start serial console
Serial.begin(115200);
//BT.begin(9600);
delay(50);
// Initiate the Wire library and join the I2C bus as a master or slave
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/**
* Set kalman and gyro starting angle
*
*/
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
// atan2 outputs the value of - to (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll);
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
//------------------------------------------------------------------------------
void loop() {
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];
// Calculate delta time
double dt = (double)(micros() - timer) / 1000000;
timer = micros();
#ifdef RESTRICT_PITCH
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
delay(2);
Serial.println();
// Corrected angles with offset
corrected_x=kalAngleX-171,746;
corrected_y=kalAngleY-81,80;
corrected_y = corrected_y+84;
Serial.print(corrected_y);
pwm_adjust(corrected_y);
if(corrected_y>=m && corrected_y<20){
if(c>6){
m-=0.2;
m1-=-0.2;
c=0;
}
backward();
}
else if(corrected_y>=-20 && corrected_y<=m1){
Serial.print(" ");
if(d>6){
m+=0.2;
m1+=0.2;
d=0;
}
forward();
}else{
stop();
m=0.7;
m1=-0.7;
pwm_on = 0;
pwm_off = 0;
}
}
void forward(){
d++;
//Serial.print(d);
digitalWrite(in3_motor_right, LOW);
digitalWrite(in4_motor_right, HIGH);
digitalWrite(in1_motor_left, HIGH);
digitalWrite(in2_motor_left, LOW);
delay(pwm_on);
digitalWrite(in3_motor_right, LOW);
digitalWrite(in4_motor_right, LOW);
digitalWrite(in1_motor_left, LOW);
digitalWrite(in2_motor_left, LOW);
delay(pwm_off);
}
void backward(){
c++;
digitalWrite(in3_motor_right, HIGH);
digitalWrite(in4_motor_right,LOW);
digitalWrite(in1_motor_left, LOW);
digitalWrite(in2_motor_left, HIGH);
delay(pwm_on);
digitalWrite(in3_motor_right, LOW);
digitalWrite(in4_motor_right, LOW);
digitalWrite(in1_motor_left, LOW);
digitalWrite(in2_motor_left, LOW);
delay(pwm_off);
}
void stop(){
digitalWrite(in1_motor_left, LOW);
digitalWrite(in2_motor_left, LOW);
digitalWrite(in3_motor_right, LOW);
digitalWrite(in4_motor_right, LOW);
delay(pwm_on);
digitalWrite(in1_motor_left, LOW);
digitalWrite(in2_motor_left, LOW);
digitalWrite(in3_motor_right, LOW);
digitalWrite(in4_motor_right, LOW);
delay(pwm_off);
}
void pwm_adjust(int value_y){
if(value_y >=-1 && value_y <=1 ){
int k = (value_y*value_y);
Serial.print(k);
pwm_on = 5; // ms ON
pwm_off = 3; // ms OFF
}
else if((value_y>=-3 && value_y<-1)||(value_y>1 && value_y<=3) ){
pwm_on = 50;
pwm_off = 5;
}
else if(value_y >5 || value_y <=-5 ){
Serial.print("**");
pwm_on = 120; // ms ON
pwm_off = 3; // ms OFF
}
else
stop();
}