}

/***********************************************************************

* Project           : Selbst-balancierendes Fahrrad

*

* Program name      : control_motor_7.8

*

* Author            : Felipe Castaneda

*                     Paula Estrada

*                     Elena Pinilla

*                     Diego Sandoval

*

* Date created      : 27.06.2017

*

* Purpose           : Movement of a mass installed on a bike according to the

* inclination angle of the bicycle itself, using a BNO055 sensor, an Arduino 101,

* Arduino Motor Shield R3, a couple of switch and a DC motor. The mass will move against the inclination

* of the bicycle according to the data obtained from the sensor. When the mass

* reach the limit on a side, it moves forward.

**********************************************************************/

//include libraries for communication with BNO055 and maths for calculation

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55);

//configuration of LED for visual recognition of several steps on the program

const int minusviel =  0;

const int minus =  1;

const int ledGrun =  4;

const int plus =  6;

const int plusviel =  7;

//configuration of switch

int schalter_link = 2;

int schalter_recht = 5;

int schalter_mitte = 10;

int tasterLink = 0;

int tasterRecht = 0;

int tasterMitte = 0;

//global variables

int inicio=0;

int centro=0;

//subroutine blink green LED

void blinkledGrun(void)

{

  digitalWrite(ledGrun, HIGH);

  delay(500);

  digitalWrite(ledGrun, LOW);

  delay(500);

  digitalWrite(ledGrun, HIGH);

  delay(500);

  digitalWrite(ledGrun, LOW);

  return;

  }

//subroutine blink 2 LED

void blinkviel(void)

{

  digitalWrite(minusviel, HIGH);

  digitalWrite(plusviel, HIGH);

  delay(500);

  digitalWrite(minusviel, LOW);

  digitalWrite(plusviel, LOW);

  delay(500);

  digitalWrite(minusviel, HIGH);

  digitalWrite(plusviel, HIGH);

  delay(500);

  digitalWrite(minusviel, LOW);

  digitalWrite(plusviel, LOW);

  return;

  }

//subroutine for motor centering

void rutine_motor(void)

{

digitalWrite(8, LOW);

digitalWrite(13, HIGH);

delay(3000);

digitalWrite(13, LOW);

while (tasterMitte!=1)

{

  tasterMitte=digitalRead(schalter_mitte);

  delay(1);

  }

digitalWrite(8, HIGH);

inicio=1;

return;

}

//show values of the sensor in serial monitor

void serial_print(void)

{

  sensors_event_t event;

  bno.getEvent(&event);

  Serial.print("X: ");

  Serial.print(event.orientation.x, 4);

  Serial.print("\tY: ");

  Serial.print(event.orientation.y, 4);

  Serial.print("\tZ: ");

  Serial.print(event.orientation.z, 4);

  Serial.println("");

  return;

}

// main program

void setup(void)

{

  Serial.begin(9600);

  //configuration LED

  pinMode(minusviel, OUTPUT);

  pinMode(minus, OUTPUT);

  pinMode(ledGrun, OUTPUT);

  pinMode(plus, OUTPUT);

  pinMode(plusviel, OUTPUT);

  //configuration switch

  pinMode(schalter_link, INPUT);

  pinMode(schalter_recht, INPUT);

  pinMode(schalter_mitte, INPUT);

  //configuration motor channel

  pinMode(8, OUTPUT); //Initiates Brake Channel B pin

  pinMode(13, OUTPUT); //Initiates Motor Channel B pin

  //configuration serial port

  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* 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);

  }

  delay(1000);

  bno.setExtCrystalUse(true);

}

void loop(void) {

  //start the motor with full power

  analogWrite(11, 255);

  //obtain value of switch

  tasterLink = digitalRead(schalter_link);

  tasterRecht = digitalRead(schalter_recht);

  tasterMitte = digitalRead(schalter_mitte);

  // Get a new sensor event  

  sensors_event_t event;

  bno.getEvent(&event);

//rutine moving weight to center

if(inicio==0){

blinkledGrun();   //show to user the initialization of the program

rutine_motor();   //start the motor subroutine

blinkviel();      //show to user in which step is the program

delay(500);

}

serial_print; //send values of the sensor to serial monitor

//when the initialization is done, the routine of the movement of the motor according to the angle is possible

if(inicio==1){

  digitalWrite(8, LOW);

  if(event.orientation.z <(double)7)

  {

  digitalWrite(minusviel, LOW);

  digitalWrite(minus, LOW);

  digitalWrite(ledGrun, LOW);

  digitalWrite(plus, LOW);

  digitalWrite(plusviel, HIGH);

  digitalWrite(13, HIGH); //change rotor direction

    }

  else if  (event.orientation.z >(double)7  && event.orientation.z <(double)12){

  digitalWrite(minusviel, LOW);

  digitalWrite(minus, LOW);

  digitalWrite(ledGrun, LOW);

  digitalWrite(plus, HIGH);

  digitalWrite(plusviel, LOW);

  digitalWrite(13, HIGH);

  }

  else if (event.orientation.z >(double)12 && event.orientation.z <(double)13)

  {

   digitalWrite(minusviel, LOW);   

   digitalWrite(minus, LOW);

   digitalWrite(ledGrun, HIGH);

   digitalWrite(plus, LOW);

   digitalWrite(plusviel, LOW);

    digitalWrite(8, HIGH);

  }

  else if (event.orientation.z >(double)13 && event.orientation.z <(double)18)

  {

    digitalWrite(minusviel, LOW);   

    digitalWrite(minus, HIGH);

    digitalWrite(ledGrun, LOW);

    digitalWrite(plus, LOW);

    digitalWrite(plusviel, LOW);

    digitalWrite(13, LOW);

  }

    else if (event.orientation.z >(double)18)

  {

    digitalWrite(minusviel, HIGH);   

    digitalWrite(minus, LOW);

    digitalWrite(ledGrun, LOW);

    digitalWrite(plus, LOW);

    digitalWrite(plusviel, LOW);

    digitalWrite(13, LOW);

  }

  delay(100);

  //reaction to positive side when motor click a switch

  if (tasterRecht == 1 )

  {

    digitalWrite(13, LOW);

    delay(200);

    }

  if (tasterLink == 1 ) //2

  {

  digitalWrite(13, HIGH);

  delay(200);}

  }

 }