Saturday, June 23, 2012

Read RX Channels with Arduino

This is so easy, their is no reason you shouldnt use a remote control RX/TX(or uber cheap) in your next project :)

Connect the RX module to power(in my case +5v and GND), and then plug in each of the signal lines for each channel into a separate digital pin. Then just two lines of code:
pinMode(CHpin, INPUT); //make digital pin connected to signal a input
unsigned long CHvalue = pulseIn(CHpin, HIGH); //pulse in on HIGH, depends on what type of signal you have
That's it; CHvalue holds the value of whatever channel of the RX.

Reading PPM sum a little bit different, but still similar to the above code:


#define channumber 4 //number of ppm sum channels
int value[channumber]; //holds PPM values
#define PPMsumPin 3

void setup()
{
  Serial.begin(115200); //Serial Begin
  pinMode(PPMsumPin, INPUT); //Pin 3 as input, this is where the ppm sum pin from the RX is connected
}
void loop()
{
  while(pulseIn( PPMsumPin , LOW) < 5000){
  } //Wait for the beginning of the frame
  for(int x=0; x<=channumber-1; x++)//Loop to store all the channel position
  {
    value[x]=pulseIn( PPMsumPin, LOW); //LOW depends on what type of signal you have
    Serial.print(" CH");
    Serial.print(x);
    Serial.print(": ");
    Serial.print(value[x]); //Print the value
    value[x]=0; //Clear the value after is printed
  }
  Serial.println(""); //Start a new line
}

Monday, June 11, 2012

STM32f0 Discovery Board with Keil

I applied for and got a STM32F0DISCOVERY board from STmicro which is based on the STM32F051R8 ARM Cortex-M0 MCU and ST-Link. I didn't really expect STmicro to actual give me one of these for free, as i am a hobbyist, and was amazed when it came in the mail; I didn't even know what i was going to use it for :).

The first the thing I needed was a IDE. I was going for a open source portable option like Eclipse with a Code Sourcery G++ compiler, but i could never get the tutorial to work! Some other free good IDEs are FreeRTOS, CoCox, and Keil AMR. I first tried Keil first, and it works fairly well, so, i think i'll keep it. Whatever ARM you have, its always good to get the ST-Link Utility(if you have a ST-Link), the standard peripheral library, and the example package(those links are for the STM32F051R8).

Keil Setup
First you need to download and install it. It's as simple as that; no tinkering with compilers etc. Then you need to create a new project by clicking Project-> New uVision Project. Then create a new folder to hold your project files; in there is where you save the new uVision project file from dialog that is opened when you click new project. Select the processor you use in the window that pops up next.

Next, it will ask if you want to import the startup file; you do.

Next, you go to the project folder and create a main.c file and add  it to the project by right clicking on Source Group 1 and clicking "Add files to..." and select main.c

Then, in the standard peripheral, or discovery firmware project, folder copy the files inside the include and source folders in Libraries\CMSIS\Device\ST\STM32F0xx\ to the Keil project folder; then import the .c file like you did main.c before. This is the bare minimum you need to program and so now you can go to main .c and write code(haven't got this part yet :)).

If you want more features you can go to the standard peripheral folder\Libraries\STM32F0xx_StdPeriph_Driver and add all the files inside inc and src to your project folder and then add all the .c to Source Group 1 and clicking "Add files to..." for all the .c files.

Rather than dealing with all of this stuff, i just like to open the STM32F0-Discovery_FW_V1.0.0\Project\Demonstration\MDK-ARM project(it already has all the .h and .c files in it) and then delete all the code in main.c and put my own there.

Programming
To program the discovery program you first press the build button(red box below). then upload it by pressing the program button(blue box below).

If you have a brand new discovery board, it will ask you to update the firmware, but when you click ok, it says "not in DFU mode". Unplug your board, takeout the two jumpers on the top right(when USB is at the top), and then plug it back in and then attempt to upload again. It should work now with no errors. Then unplug the board and put the jumpers back on.

Some times it doesn't work and it just sits there at Load "..." in the info window at the bottom of the Keil program. To fix this click the green boxed button; check that you have the correct device selected in the Device tab, and then go to Utilities and makes sure ST-Link is selected(or ST-Link depreciated if you haven't updated the firmware yet). But it still may not work; now go to the Utilities tab and click on settings(like in the second pic below). You see that mine has a programming algorithm in there; if your window doesn't have this, go to add and find your board. Now it should program correctly.



Now i need to figure out how to write code for this thing; its different than Arduino, not because of its syntax(C is almost like Arduino Wire), but because of the "micro syntax"-- AKA the code to change GPIO registers, use I2C etc.

Sunday, June 3, 2012

Pololu MiniIMU-9 Code

I finally got around to finishing some code for my Arduino that interfaces with the Pololu MiniIMU9 to get gyro, accelerometer and magnetometer data. But, that would be boring alone and so i implemented pitch, roll and yaw(with mag), too. Also, I somehow managed to successfully implement Kalman filtering to combine gyro and acc data. But, i did have a few issues as i was writing this code with setting up the Wire interface and calculating pitch/roll/yaw from gyro, acc and mag data.

Hopefully, with this post i will explain how i did this so that others can apply it to other IMUs etc. First thing you need to do is enable the sensors through the registers. Most times this enable register (often the first register) is named similar to mine:  CTRL_REG1. Looking the datasheet for the sensor, you can find what value this register needs to be for your startup settings. This is how you send values to registers:
Wire.beginTransmission(address);
Wire.write(register);
Wire.write(value);
Wire.endTransmission();

In my code they were made into functions:
writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)

writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)

writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

Next, you need to look for sensitivity; it will given for different modes (2000dps, 1000dps etc) for the gyro so you need to use the sensitivity factor that lines up with the dps. Same for AccGain, if you use 8 gauss mode, you need to use the sensitivity for that mode. This code is how i made sure my gyro/acc was on the right mode so that my Gyro/AccGain lined up in the calculation part of the code(explained later):

//gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled
So it seems that registers 1(enable, data rate, filters) and 4(update mode, sensitivity etc) are the most important. I still need to figure out how to choose the best  register settings for ODR and high and low pass filters.

The next thing you need to do is get some values from the sensors so that you can determine the level position values and use that as a bias throughout the code. Simply get xyz values from the Acc and Gyro a 100 times, or so, and then divide by 100 to get an average.

But, you need to read the values from the sensors first, and to do that something similar to enabling the sensors is used. It first begins the communication with the sensor, and then writes a value to the output register signalling that it wants data. Then it waits for the request data and then moves them to bytes. The bytes are then recombined into whole numbers into integers of the x/y/z axis.

Wire.beginTransmission(address);
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(address, 6);

  while (Wire.available() < 6);

  byte xla = Wire.read();
  byte xha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;

Now that you have the data into easy to work with integers, you can calculate the pitch, roll, and yaw etc. That in its self is easy; all you have to do is: pitchGyro = (GYROx - bGYROx) / GyroGain; or rollGyro = (GYROy - bGYROy) / GyroGain; but that wont provide very accurate results. GYROx is the value received from the gyro, bGYROx is the bias for the gyro calculated when the IMU was first turned on, and GyroGain is the factor that gives the value in DEG/s from the raw sensor value. This can be found in the data sheet as i mentioned before.

To provide accurate results, you need to combine acc and gyro values with a kalman filter. The Kalman filter has two main parts. One calculates pitch and roll individually based on acc and gyro:
Gyro pitch = (Gyro pitch  + ((GyroXvalue -  GyroXbias) / GyroGain)) * timeStep;

Accel pitch = (atan2((AccYvalue -  AccYbias) / AccGain, (AccZvalue  -  AccZbias) / AccGain) * 180.0) / PI;
Pitch prediction = Pitch prediction + ((GyroXvalue -  GyroXbias) / GyroGain) * timeStep;

Gyro roll = (rollGyro + ((GyroYvalue -  GyroYbias) / GyroGain)) * timeStep; //gyro roll in deg
Acc roll = (atan2(( AccXvalue -  AccXbias ) / AccGain, (AccZvalue -  AccZbias) / AccGain) * 180.0) / PI;
Roll prediction = rollPrediction - ((GyroYvalue -  GyroYbias) / GyroGain) * timeStep;
timestep is  how long one loop of the program is, and (*180.0) / PI is converting radians into degrees.

and the other, which is the actual filter which combines the gyro and acc data to get pitch/roll. i dont really know much about this, so i cant really explain.
void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

Next up is to calculate yaw accurately with the help of the mag. MAGx/y/z are the raw values from the sensor.

void YawCalc() // calculate yaw from mag
{
  //YAW!
  //this part is required to normalize the magnetic vector
  //get Min and Max Reading for MAGic Axis
  if (MAGx>xMAGMax) {
    xMAGMax = MAGx;
  }
  if (MAGy>yMAGMax) {
    yMAGMax = MAGy;
  }
  if (MAGz>zMAGMax) {
    zMAGMax = MAGz;
  }

  if (MAGx<xMAGMin) {
    xMAGMin = MAGx;
  }
  if (MAGy<yMAGMin) {
    yMAGMin = MAGy;
  }
  if (MAGz<zMAGMin) {
    zMAGMin = MAGz;
  }

  //Map the incoming Data from -1 to 1
  xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
  yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
  zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

  //normalize the magnetic vector
  float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
  xMAGMap /=norm;
  yMAGMap /=norm;
  zMAGMap /=norm;

  //new calcs:
  float xh = xMAGMap * cos(pitchPrediction) + zMAGMap * sin(pitchPrediction);
  float yh = xMAGMap * sin(rollPrediction) * sin(pitchPrediction) + yMAGMap * cos(rollPrediction) - zMAGMap * sin(rollPrediction) * cos(pitchPrediction);
  float zh = -xMAGMap * cos(rollPrediction) * sin(pitchPrediction) + yMAGMap * sin(rollPrediction) + zMAGMap * cos(rollPrediction) * cos(pitchPrediction);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }
}

Code:


#include <Wire.h>

//I2C addressses for IMU
#define GYR_ADDRESS (0xD2 >> 1)
#define MAG_ADDRESS (0x3C >> 1)
#define ACC_ADDRESS (0x30 >> 1)

//Gyro, ACC, Mag enable registers
#define L3G4200D_CTRL_REG1 0x20
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_MR_REG_M 0x02

//acc settings
#define CTRL_REG4_A 0x23

//gyrto settings
#define CTRL_REG4 0x23

//Gyro, Acc, Mag output registers
#define L3G4200D_OUT_X_L 0x28
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_M 0x03

//ACC Reg data
int ACCx;
int ACCy;
int ACCz;
//Bias
int bACCx;
int bACCy;
int bACCz;

//Gyro Reg data
int GYROx;
int GYROy;
int GYROz;
//Bias
int bGYROx;
int bGYROy;
int bGYROz;

//MAG REG data
int MAGx;
int MAGy;
int MAGz;

//YAW calc
int xMAGMax;
int yMAGMax;
int zMAGMax;
int xMAGMin;
int yMAGMin;
int zMAGMin;
float xMAGMap;
float yMAGMap;
float zMAGMap;


//calculations
int pitchAccel;
int pitchGyro;
int rollAccel;
int rollGyro;
//float YawU;

//gyro/acc gain-converts raw values, gyro deg/s, acc to Gs
#define AccGain 3.9      //8g
#define GyroGain 70     //2000dps

//kalman,
float giroVar = 0.1;
float deltaGiroVar = 0.1;
float accelVar = 5;
float Pxx = 0.1; // angle variance
float Pvv = 0.1; // angle change rate variance
float Pxv = 0.1; // angle and angle change rate covariance
float kx, kv;

//final values
float pitchPrediction = 0; //Output of Kalman filter, final pitch value
float rollPrediction = 0;  //Output of Kalman filter, final roll value
float yawRaw=0; //final yaw value. yaw doesnt go through kalman! has its own calc.

//time, used in kalman filtering to keep a constant loop time
unsigned long timer = 0;
unsigned long timer1 = 0;
float timeStep = 0.02;          //20ms. Need a time step value for integration of gyro angle from angle/sec

void setup()
{
  Serial.begin(115200); //begin serial comm.

  Wire.begin(); //start Wire for IMU

  writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)
  writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)
  writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

  //gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled

  CalibrateIMU(); //calibrate the IMU for level starting pos.
}

void loop() {
  timer = millis(); //loop begin time

  //read sensors
  readGyro();
  readAcc();
  readMag();

  //calcualte pitch, roll, yaw, kalman etc
  Calculations();

  //print values
  PrintVals();

  timer1 = millis(); //loop end time
  delay(((timeStep * 1000)-(timer1-timer))); //delay so loop lasts 20msec, (timestep(.02) * 1000 = msec) - how long loop took
}

void readGyro() // get x, y, z values from gyro
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(L3G4200D_OUT_X_L | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(GYR_ADDRESS, 6);

  while (Wire.available() < 6);

  uint8_t xla = Wire.read();
  uint8_t xha = Wire.read();
  uint8_t yla = Wire.read();
  uint8_t yha = Wire.read();
  uint8_t zla = Wire.read();
  uint8_t zha = Wire.read();

  GYROy = xha << 8 | xla;
  GYROx = yha << 8 | yla;
  GYROz = zha << 8 | zla;
}

void readAcc() // get x, y, z values from accelerometer
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(ACC_ADDRESS, 6);

  while (Wire.available() < 6);

  byte xla = Wire.read();
  byte xha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;
}

void readMag() //get mag x, y, z values
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303_OUT_X_H_M);
  Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 6);

  while (Wire.available() < 6);

  byte xhm = Wire.read();
  byte xlm = Wire.read();

  byte yhm, ylm, zhm, zlm;

  zhm = Wire.read();
  zlm = Wire.read();
  yhm = Wire.read();
  ylm = Wire.read();

  MAGx = (xhm << 8 | xlm);
  MAGy = (yhm << 8 | ylm);
  MAGz = (zhm << 8 | zlm);
}

void CalibrateIMU() //get level value bias of IMU sensors
{
  //temporary total holders
  int tGYROx;
  int tGYROy;
  int tGYROz;

  int tACCx;
  int tACCy;
  int tACCz;

  delay(100); //wait for stable values
  for(int i = 0; i<50; i++) //get values fifty times for acc + gyro
  {
    readGyro();
    readAcc();
    readMag();

    tGYROx += GYROx; //total gyrox value += current reading
    tGYROy += GYROy;
    tGYROz += GYROz;

    tACCx += ACCx;
    tACCy += ACCy;
    tACCz += ACCz;
    delay(8);
  }

  bGYROx = tGYROx / 50; //bias in gyro x = total gyro x/50
  bGYROy = tGYROy / 50;
  bGYROz = tGYROz / 50;

  bACCx = tACCx / 50;
  bACCy = tACCy / 50;
  bACCz = (tACCz / 50) - 256; //Don't compensate gravity away! We would all (float)!
}


void Calculations() //calculate roll/pitch for acc/gyro, remove level bias. kalman filtering for pitch/roll, calc yaw
{
  /*
  Gyro in deg/s
   pitchGyro = (GYROx - bGYROx) / GyroGain;
   rollGyro = (GYROy - bGYROy) / GyroGain;
   */

  pitchGyro = (pitchGyro + ((GYROx - bGYROx) / GyroGain)) * timeStep; //gyro pitch in deg
  pitchAccel = (atan2((ACCy - bACCy) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  pitchPrediction = pitchPrediction + ((GYROx - bGYROx) / GyroGain) * timeStep;

  rollGyro = (rollGyro + ((GYROy - bGYROy) / GyroGain)) * timeStep; //gyro roll in deg
  rollAccel = (atan2((ACCx - bACCx) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  rollPrediction = rollPrediction - ((GYROy - bGYROy) / GyroGain) * timeStep;

  YawCalc();  //calc yaw with mag!

  Kalman(); //predict pitch, roll
}

void YawCalc() // calculate yaw from mag
{
  //YAW!
  //this part is required to normalize the magnetic vector
  //get Min and Max Reading for MAGic Axis
  if (MAGx>xMAGMax) {
    xMAGMax = MAGx;
  }
  if (MAGy>yMAGMax) {
    yMAGMax = MAGy;
  }
  if (MAGz>zMAGMax) {
    zMAGMax = MAGz;
  }

  if (MAGx<xMAGMin) {
    xMAGMin = MAGx;
  }
  if (MAGy<yMAGMin) {
    yMAGMin = MAGy;
  }
  if (MAGz<zMAGMin) {
    zMAGMin = MAGz;
  }

  //Map the incoming Data from -1 to 1
  xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
  yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
  zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

  //normalize the magnetic vector
  float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
  xMAGMap /=norm;
  yMAGMap /=norm;
  zMAGMap /=norm;

  //new calcs:
  float xh = xMAGMap * cos(pitchPrediction) + zMAGMap * sin(pitchPrediction);
  float yh = xMAGMap * sin(rollPrediction) * sin(pitchPrediction) + yMAGMap * cos(rollPrediction) - zMAGMap * sin(rollPrediction) * cos(pitchPrediction);
  float zh = -xMAGMap * cos(rollPrediction) * sin(pitchPrediction) + yMAGMap * sin(rollPrediction) + zMAGMap * cos(rollPrediction) * cos(pitchPrediction);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }
}

void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

//write stuff to the snsor registers
void writeGyroReg(byte reg, byte value)
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeAccReg(byte reg, byte value)
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeMagReg(byte reg, byte value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void PrintVals()
{
  /*
  Serial.print("RawPitchGyro:");
   Serial.print(pitchGyro);
   Serial.print(" RawPitchAccel:");
   Serial.print(pitchAccel);
   Serial.print(" RawRollGyro:");
   Serial.print(rollGyro);
   Serial.print(" RawRollAccel:");
   Serial.print(rollAccel);
   Serial.print(" PitchPredict:");
   Serial.print(pitchPrediction);
   Serial.print(" RollPredict:");
   Serial.print(rollPrediction);
   Serial.print(" Yaw:");
   Serial.print(yawRaw);
   Serial.println();
   */

  //IMU grapher processing
  Serial.print(map(pitchPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(yawRaw, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchGyro, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchAccel, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollGyro, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollAccel, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(pitchPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(yawRaw, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollGyro, -180,180,0,500));
  Serial.println();

}


Next is to implement some PID and make some balance bot magic!