I use a Arduino nano 33 ble as a custom Headtracker to track Yaw,Pitch Roll. With those information i want to controll the pos of 3 servos but after i integrated the funktion method "schreiben" its stops working for some reason. I think there is something wrong how i write the servos and the map funktion. Reading Yaw,Pitch and Roll isnt a problem.
Any help is much appreciated.
#include <Arduino_LSM9DS1.h>
#include "MadgwickAHRS.h"
#include <Servo.h>
Servo myservoyaw;
Servo myservopitch;
Servo myservoroll;
int i;
int pos1 ;
int pos2 ;
int pos3 ;
float roll, pitch, heading;
// initialize a Madgwick filter:
Madgwick filter;
// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;
void setup() {
Serial.begin(9600);
// attempt to start the IMU:
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU");
// stop here if you can't access the IMU:
while (true);
}
// start the filter to run at the sample rate:
filter.begin(sensorRate);
myservoyaw.attach(D9);
myservopitch.attach(D8);
myservoroll.attach(D7);
}
void loop() {
// values for acceleration and rotation:
float xAcc, yAcc, zAcc;
float xGyro, yGyro, zGyro;
// check if the IMU is ready to read:
if (IMU.accelerationAvailable() &&
IMU.gyroscopeAvailable()) {
// read accelerometer &and gyrometer:
IMU.readAcceleration(xAcc, yAcc, zAcc);
IMU.readGyroscope(xGyro, yGyro, zGyro);
// update the filter, which computes orientation:
filter.updateIMU(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc);
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
Serial.print("Orientation: ");
Serial.print(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
Serial.print(pos1);
Serial.print(pos2);
Serial.println(pos3);
schreiben();
}
}
void schreiben(){
if( i>=1000 ){
pos1 = map(heading, 0, 360, 0, 179);
pos2 = map(pitch, -88, -88, 0, 179);
pos3 = map(roll, -176, 176, 0, 179);
myservoyaw.write(pos1);
myservopitch.write(pos2);
myservoroll.write(pos3);
i = 0;
}
else{
i++;
}
}