Удалось управлять светодиодом - отлично.
Теперь присмотримся к loop() балансира:
Код:
void loop() {
/* Update all the values */
uint8_t* data = i2cRead(0x3B,14);
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
gyroY = ((data[10] << 8) | data[11]);
/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
double gyroYrate = -((double)gyroY/131.0);
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
in = kalAngleY;
pid.Compute();
if (out < 0) {
pwm = -1 * out;
analogWrite(11, 0);
analogWrite(10, pwm);
} else {
pwm = out;
analogWrite(10, 0);
analogWrite(11, pwm);
}
delay(1); // The accelerometer's maximum samples rate is 1kHz
}
Команда
pid.Compute() рассчитала переменную
out, от которой зависит движение робота. Можно после этой команды вставить корректировку её значения, чтобы качать балансер в нужную сторону. Заведем специальную переменную, чтобы хранить число итераций, и переменную для приращения. Там, где в заголовке есть строчка
double in, out, setpoint; добавим строчки
double delta;
int iter;Они нам нужны чтобы корректура длилась некоторое время.
Код:
if (Serial.available()) {
int inByte = Serial.read();
if (inByte=='1') {
delta=10; // Приращение
iter=50; // Итерации
}
if (inByte=='0') {
delta=-10;
iter=50;
}
}
if (iter>0) {
iter--;
out=out+delta;
}
В сетапе все прекрасно, но скорость обмена высоковата. НС-05, вроде бы, держит 9600, а не 115200. Меняем.
Балуемся переменными. Публикуем результаты опытов.
Вход
Регистрация
FAQ
Поиск
