1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
|
/*
AnalogReadSerial
Reads an analog input on pin 0, prints the result to the serial monitor.
Attach the center pin of a potentiometer to pin A0, and the outside pins to +5V and ground.
This example code is in the public domain.
*/
#include <Servo.h>
// Pin used in this example
#define SERVO 9
#define X_GYRO 0
#define Y_GYRO 1
#define ADCresolution 4.89f // = 5000mV/1023counts: Arduino analog pins resolution expressed in mV/count
#define Sensitivity 0.67f // [mV/dps] sensitivity of the sensor, took from datasheet (4x output mode)
// Conversion coefficient, we do here because is a constant! so we'll not do the calculation every loop
#define K ADCresolution/Sensitivity // the constant!
#define nrSamples 6
Servo myservo;
// Timing variables
unsigned long time, sampleTime = 12;
unsigned long printTime = 0, serialRefresh_time = 500;
float deltaT = (float)sampleTime*nrSamples/1000;
//Gyroscope variables
int roll_zeroVoltage, pitch_zeroVoltage;
int roll_rawADC[nrSamples], pitch_rawADC[nrSamples]; // store 3 values...just to avverage
float roll_rate, pitch_rate; //
float roll_angle = 0, pitch_angle = 0;
int c=0; // just a counter to count the samples
int pos; // variable to store the servo position
int filterGyro(int buffer[]);
// the setup routine runs once when you press reset:
void setup()
{
// initialize serial communication at 9600 bits per second:
Serial.begin(115200);
}
// the loop routine runs over and over again forever:
void loop()
{
// read the input on analog pin 0:
//int sensorValue = analogRead(A2), analogRead(A3);
{
// Every 40ms take a sample from gyro
if(millis() - time > sampleTime)
{
time = millis();
roll_rawADC[c] = analogRead(Y_GYRO);
pitch_rawADC[c] = analogRead(X_GYRO);
c++;
}
if(c==nrSamples) // Well, we have 3 samples
{
// Transform the raw data into an angular velocity
roll_rate = (filterGyro(roll_rawADC) - roll_zeroVoltage) * K;
pitch_rate = (filterGyro(pitch_rawADC) - pitch_zeroVoltage)* K;
roll_angle += roll_rate *deltaT/2;
pitch_angle += pitch_rate*deltaT/2;
//Keep our angle between 0-359 degrees
if (roll_angle < 0)
roll_angle += 360;
else if (roll_angle > 359)
roll_angle -= 360;
if (pitch_angle < 0)
pitch_angle += 360;
else if (pitch_angle > 359)
pitch_angle -= 360;
// Now we control the servo: home position is setted in the center at 90 degrees
if(roll_angle >= 0 && roll_angle <= 90) // counterclockwise rotation of the gyro...
pos = 90 + (int)roll_angle; // ...produces rotation from 90 to 180 deg on servo
if(roll_angle >= 270) // clockwike rotation of the gyro...
pos = (int)roll_angle - 270; // ...produces rotation from 90 to 0 deg on servo
myservo.write(pos); // send the position to servo
if(millis() - printTime > serialRefresh_time)
{
printTime = millis();
Serial.print("Roll speed: ");
Serial.print((int)roll_rate);
Serial.print("\t Angle: ");
Serial.print((int)roll_angle);
Serial.print("\t Pitch speed: ");
Serial.print((int)pitch_rate);
Serial.print("\t Angle: ");
Serial.println((int)pitch_angle);
Serial.print("Servo: ");
Serial.println(pos);
}
// print out the value you read:
//Serial.println(sensorValue);
delay(1500); // delay in between reads for stability
}
int filterGyro(int buffer[]);
{
int mean=0;
for(byte i=0; i<nrSamples; i++)
//mean += buffer(i);
mean /= nrSamples;
//return mean;
}
}
}
|