Balancingbot : Différence entre versions

De troyesGEII
Aller à : navigation, rechercher
(Page créée avec « ={{Rouge|Premiere partie}}= »)
 
(Ajout d'un code)
Ligne 1 : Ligne 1 :
  
 
={{Rouge|Premiere partie}}=
 
={{Rouge|Premiere partie}}=
 +
 +
Code non-fonctionnel
 +
 +
<source lang=c>
 +
#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();
 +
  }
 +
}
 +
</source>

Version du 6 janvier 2017 à 11:17

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();
  }
}