Balancingbot

Premiere partie

Code non-fonctionnel

#include "Wire.h"
#include "SPI.h"  
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "I2Cdev.h"
#include "MPU6050.h"

#define MAX_READ_ABS 16384
#define MAX_WRITE_ABS 255
#define OFFSET -2000

const double MAX_READ_NEG = (-MAX_READ_ABS+OFFSET/2.0);
const double MAX_READ_POS = (MAX_READ_ABS+OFFSET/2.0);

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

long accelero;

void setup()
{
  Wire.begin();
  accelgyro.initialize();
  for(unsigned char i = 2 ; i <= 5 ; i++)
  {
    pinMode(i, OUTPUT);
  }
  Serial.begin(9600);
}

long getAccelero(long n = 100)
{
  long value = 0;
  for(long i = 0 ; i < n ; i++)
  {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    value += ay;
  }  
  return value/n;
}

void arreter()
{
  digitalWrite(2, LOW);
  digitalWrite(4, LOW);
}

void avancer(char n = 50)
{
  if(n == 0)
  {
    arreter();
  }
  else
  {
    analogWrite(3,(unsigned char)(MAX_WRITE_ABS - MAX_WRITE_ABS*(100-n)/200.0));
    analogWrite(5,(unsigned char)(MAX_WRITE_ABS - MAX_WRITE_ABS*(100-n)/200.0));
    digitalWrite(2,HIGH);
    digitalWrite(4,HIGH);
  }
}

void reculer(char n = 50)
{
  if(n == 0)
  {
    arreter();
  }
  else
  {
    analogWrite(3,(unsigned char)(MAX_WRITE_ABS*(100-n)/200.0));
    analogWrite(5,(unsigned char)(MAX_WRITE_ABS*(100-n)/200.0));
    digitalWrite(2,HIGH);
    digitalWrite(4,HIGH);
  }
}

void tourner(char n = 0)
{
  
}

void loop()
{
  bool _stop = 0;
  
  accelero = getAccelero(1);
  Serial.println(accelero);
  if(_stop)
  {
    if(accelero > 0.9*MAX_READ_POS || accelero < 0.9*MAX_READ_NEG)
    {
      arreter();
    }
    else
    {
      if(accelero < OFFSET)
      {
        reculer(accelero/(0.9*MAX_READ_NEG)*100);
      }
      else
      {
        avancer(accelero/(0.9*MAX_READ_POS)*100);
      }
    }
  }
  else
  {
    arreter();
  }
}