PA_0021.ino

/**************************************************************************/
/*
 F1E-Steering with Sensor Adafruit BNO055 (Orientation-Fusion in sensor) 
 Moves the serco if there is a deviation to desired/wanted direction 
 tested with UNO and NANO Boards
 written for ARDUINO
*/
/**************************************************************************/
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <LiquidCrystal_I2C.h>

// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);


//#include <Servo.h> // use of the PWM-library
#include <Adafruit_TiCoServo.h> 

// Set the delay between fresh samples /
#define BNO055_SAMPLERATE_DELAY_MS (100)
// Check I2C device address from BNO055 and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);

// Variables for sensor evaluation
float roll, pitch, heading;
int delayTime=500;

// Variables for direction change / F1E_Servo_Move ()
int current_Bearing = 180;                          // INT-Variable for current heading
int wanted_Bearing = 180;                           // INT-Variable for desired heading
int delta_Bearing;                                  // INT-(+-)Variable for deviation from desired heading

// Servo Variables
//int servoPinF1E     =  5;  // Servo Pin for  <Servo.h> libray. Unfortunately servos jitter with this library (communication delay with I2C) 
int servoPinF1E     =  9;    // Servo Pin for <Adafruit_TiCoServo.h> library to avoid servo jitters https://forums.adafruit.com/viewtopic.php?f=22&t=96847 
                          // https://learn.adafruit.com/neopixels-and-servos/the-ticoservo-library
int minGrad      =  45;   // minimum servo position  45°
int maxGrad     =  135;   // maximum servo position  135°
int centerGrad  = 90;     // midle position Servos
int turnGrad    = 1;      // Deltal degrees for degrees-change
int winkelDelay = 1;      // Pause for signal evaluation
int servoPos;             // Servo position

int servoPinDT    =   10;   // Dummy Servo for later use
int servoPosDT;             // Servo position for later use

// Servo-Trimmungs-Varianten
const int potiPin = A0;   // Analog-Input for Trimpoti
int sensor = 0;           // sensor-Value on Analog-Input (0 - 1023) 
int pwm = 0;              // calculated sensor-value for the PWM-Trim-Value (-maxTrim/2 bis + maxTrim/2)
int pwmOld = 0;           // PWM-value to remember for next loop
int maxTrim = 40;         // max. Trim-degrees

//Servo servoF1E; 
Adafruit_TiCoServo servoF1E; 
Adafruit_TiCoServo servoDT; 
// Button Variable
int SW1          =  8;    // Input button / digital pin:  input for desired heading and trimming 
int SW2          =  7;    // Input button / digital pin:  input for DT-Timer-Function for later use 

//PID-correction
int abs_delta_Bearing = 0; 
int abs_delta_Bearing_n1 = 0; // Vorbelegung für die vergangene Abweichung 1 Messung vorher
int abs_delta_Bearing_n2 = 0; // Vorbelegung für die vergangene Abweichung 2 Messungen vorher

int PID_Input;
int PID_Output;

float sensor_P;
float sensor_I;
float sensor_D;

float PID_Kp = 1.0;
float PID_Ki = 0;
float PID_Kd = 0;

int PID_potiPin = A1; 

void setup() {
  // initialize the LCD
  lcd.begin();

  // Turn on the blacklight and print a message.
  lcd.backlight();
  lcd.print("F1E-Sensor-program");
 
 /* Initialise serial Com Board*/
 Serial.begin(115200);
  while (!Serial) yield();

  /* Initialise the internal LED for calibration Status servo */
  pinMode(LED_BUILTIN,OUTPUT);

  // Set the switch-Buttons
  pinMode(SW1,INPUT);           // define button type
  digitalWrite(SW1,HIGH);       // button default High
  pinMode(SW2,INPUT);           // define button type
  digitalWrite(SW2,HIGH);       // button default High
  
  /* Initialise the servo */
  servoF1E.attach(servoPinF1E);    // connect Servo F1E
  servoDT.attach(servoPinDT);    // connect Servo DT
  servoPos = centerGrad;        // move servo to default midle position

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  displaySensorDetails();
  displaySensorStatus();
  displayCalStatus();
  
  servoF1E.write(minGrad);
 
  // Check calibration status in a loop. End setup only, when calibration is sufficient
  //Calibrate
  while (Analyse_CalStatus())
  {
  Serial.println("Not yet sufficiant calibrated");
  digitalWrite(LED_BUILTIN, HIGH);
  delay(delayTime);
  digitalWrite(LED_BUILTIN, LOW);
  delay(delayTime);
  }
  
  digitalWrite(LED_BUILTIN, HIGH);
  delay(2000);
  digitalWrite(LED_BUILTIN, LOW);
  F1E_Get_Current_Bearing ();
  wanted_Bearing = current_Bearing; 
  F1E_Servo_Move ();
}



void loop() {
  
  F1E_Get_Current_Bearing (); 
  F1E_Get_Wanted_Bearing (); 
  F1E_Servo_Move ();
  //DT_Interupt ();
}



/**************************************************************************/
/*
    Analyse the Calibration status 
*/
/**************************************************************************/
bool Analyse_CalStatus ()
  {
  /* Get the four calibration values (0..3) */
  /* Any sensor data reporting 0 should be ignored, */
  /* 3 means 'fully calibrated" */
  uint8_t system, gyro, accel, mag;
  system = gyro = accel = mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);

  // If the sensor is not sufficient calibrated, return 1. 
  if (gyro > 2 && mag > 2)  {return 0;}
  return 1;    
  }

/**************************************************************************/
/*
    Determine the desired bearing 
*/
/**************************************************************************/
void F1E_Get_PID_K ()
  {
 
    PID_potiPin = A1;
    sensor_P = analogRead(PID_potiPin);
    PID_Kp = sensor_P/1023;
    
    PID_potiPin = A2;
    sensor_I = analogRead(PID_potiPin);
    PID_Ki = sensor_I/10230;

    PID_potiPin = A3;
    sensor_D = analogRead(PID_potiPin);
    PID_Kd = sensor_D/(2*1023);
    Serial.print("sensor_P = ");
    Serial.print(sensor_P);
    Serial.print("PID_Kp = ");
    Serial.print(PID_Kp);
    Serial.print("|| sensor_I = ");
    Serial.print(sensor_I);
    Serial.print("PID_Ki = ");
    Serial.print(PID_Ki);
    Serial.print("|| sensor_D = ");
    Serial.print(sensor_D);
    Serial.print("PID_Kd = ");
    Serial.println(PID_Kd);
    
  }  

/**************************************************************************/
/*
    Determine the desired bearing 
*/
/**************************************************************************/
void DT_Interupt ()
{
if(!digitalRead(SW2))   
  {
  DT_Servo_Move ();
  }
}

/**************************************************************************/
/*
    Determine the desired bearing 
*/
/**************************************************************************/
void F1E_Get_Wanted_Bearing ()
  {
  if(!digitalRead(SW1)) 
    { 
    // Check if there is a change necessary for the middle position 
    sensor = analogRead(potiPin);
    pwm = map(sensor, 0, 1023, 0, maxTrim) - maxTrim/2;
    
    // new wanted bearing
    wanted_Bearing = current_Bearing;
    centerGrad = centerGrad - (pwm-pwmOld);
    minGrad=minGrad-(pwm-pwmOld);
    maxGrad=maxGrad-(pwm-pwmOld);
    pwmOld = pwm; 

    // Move servo position to the (new) middle position
    servoPos = centerGrad;
    Serial.print("Sensor = ");
    Serial.print(sensor);
    Serial.print("Centergrad = ");
    Serial.print(centerGrad);
    Serial.print(" PWM  = ");
    Serial.println(pwm);
  
    Serial.print("Direction button pressed - new direction and trim"); 
    Serial.println();
    }  
   return;
  }

/**************************************************************************/
/*
    Moves the serco if there is a deviation to desired/wanted direction 
*/
/**************************************************************************/
void F1E_PID ()
  {
  
  PID_Input = abs_delta_Bearing ; 
  // Output mit PID - Kp modifizieren 
  F1E_Get_PID_K ();
  PID_Output = PID_Kp * abs_delta_Bearing + 
               PID_Ki * (abs_delta_Bearing + abs_delta_Bearing_n1 + abs_delta_Bearing_n2) +
               PID_Kd * (abs_delta_Bearing - abs_delta_Bearing_n1); 
  }
/**************************************************************************/
/*
    Moves the F1E-servo if there is a deviation to desired/wanted direction 
*/
/**************************************************************************/
void F1E_Servo_Move ()
  {
    delta_Bearing =  wanted_Bearing - current_Bearing; 
    //Modulo for beeing aware to 0/360° change of direction and calculation only the deviation/direction for corretion  
    Serial.print("Modulo degrees change from desired direction: "); 
    
    //Remember to previous delta_bearing - they will be used for PID calculation
    abs_delta_Bearing_n2 = abs_delta_Bearing_n1;
    abs_delta_Bearing_n1 = abs_delta_Bearing;
    abs_delta_Bearing = ((delta_Bearing + 540) % 360) - 180; 
    Serial.println(abs_delta_Bearing);
    
    //servoPos = centerGrad - abs_delta_Bearing * PID_Kp;
    F1E_PID();
    servoPos = centerGrad - PID_Output;
     
    // Limitation of correction  
    if(servoPos > maxGrad) {servoPos = maxGrad;} 
    if(servoPos < minGrad) {servoPos = minGrad;} 
    //delay(winkelDelay); 
    
    // go to new Servo position
    servoF1E.write(servoPos);
    
   return;
  }
/**************************************************************************/
/*
    Moves the DT-servo if there is a deviation to desired/wanted direction 
*/
/**************************************************************************/
void DT_Servo_Move ()
  {
/*Aktuell noch Dummy Code um überhaupt was zu tun*/

       
    // go to new Servo position
    servoPosDT = 90;
    servoDT.write(servoPosDT);
    delay (1000);
    servoPosDT = 135;
    servoDT.write(servoPosDT);
    delay (1000);
    servoPosDT = 45;
    servoDT.write(servoPosDT);
    delay (1000);
  }


/**************************************************************************/
/*
    Displays some basic information on this sensor from the unified
    sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
  sensor_t sensor;
  bno.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display some basic info about the sensor status
*/
/**************************************************************************/
void displaySensorStatus(void)
{
  /* Get the system status values (mostly for debugging purposes) */
  uint8_t system_status, self_test_results, system_error;
  system_status = self_test_results = system_error = 0;
  bno.getSystemStatus(&system_status, &self_test_results, &system_error);

  /* Display the results in the Serial Monitor */
  Serial.println("");
  Serial.print("System Status: 0x");
  Serial.println(system_status, HEX);
  Serial.print("Self Test:     0x");
  Serial.println(self_test_results, HEX);
  Serial.print("System Error:  0x");
  Serial.println(system_error, HEX);
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display sensor calibration status
*/
/**************************************************************************/
void displayCalStatus(void)
{
  /* Get the four calibration values (0..3) */
  /* Any sensor data reporting 0 should be ignored, */
  /* 3 means 'fully calibrated" */
  uint8_t system, gyro, accel, mag;
  system = gyro = accel = mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);

  /* The data should be ignored until the system calibration is > 0 */
  Serial.print("\t");
  if (!system)
  {
    Serial.print("! ");
  }

  /* Display the individual values */
  Serial.print("Sys:");
  Serial.print(system, DEC);
  Serial.print(" G:");
  Serial.print(gyro, DEC);
  Serial.print(" A:");
  Serial.print(accel, DEC);
  Serial.print(" M:");
  Serial.print(mag, DEC);
}

/**************************************************************************/
/*
    determine current heading
*/
/**************************************************************************/
void F1E_Get_Current_Bearing ()
 {
 imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
 heading = euler.x();
 current_Bearing = heading; // Wandlung auf int

  /* Display the floating point data */
  Serial.print("X: ");
  Serial.print(euler.x());
  Serial.print(" Y: ");
  Serial.print(euler.y());
  Serial.print(" Z: ");
  Serial.print(euler.z());
  Serial.print("\t\t");
  
// print the heading, pitch and roll    Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(", ");
   Serial.print(current_Bearing);
  Serial.print(", ");
  Serial.print(wanted_Bearing);
  Serial.print(", ");
  Serial.print(pitch);
  Serial.print(", ");
  Serial.println(roll);
}