//Arduino 1.0+ only #include #include #define CTRL_REG1 0x20 #define CTRL_REG2 0x21 #define CTRL_REG3 0x22 #define CTRL_REG4 0x23 #define CTRL_REG5 0x24 int L3G4200D_Address = 105; //I2C address of the L3G4200D int x; int y; int z; Servo Pinky; Servo Ring; Servo Index; Servo Pointer; Servo Thumb; int val; int val1; int val2; int val3; int val4; int smoothedRotation; float filterVal; // this determines smoothness - .0001 is max 1 is off (no smoothing) float smoothedVal; // this holds the last loop value just use a unique variable for every different sensor that needs smoothing float smoothedVal1; float smoothedVal2; float smoothedVal3; float smoothedVal4; Servo WRotation; Servo WLeftRight; Servo WUpDown; void setup(){ Wire.begin(); Serial.begin(9600); Serial.println("starting up L3G4200D"); setupL3G4200D(2000); // Configure L3G4200 - 250, 500 or 2000 deg/sec delay(1500); //wait for the sensor to be ready WRotation.attach(12); WLeftRight.attach(13); WUpDown.attach(11); Pinky.attach(7); Ring.attach(6); Index.attach(5); Pointer.attach(4); Thumb.attach(3); } void loop(){ getGyroValues(); // This will update x, y, and z with new values //Serial.print("X:"); //Serial.print(x); //Serial.print(" Y:"); //Serial.print(y); delay(100); //Just here to slow down the serial to make it more readable x = map(x, -1023, 1023, 0, 179); //Serial.print(" X:"); smoothedRotation = smoothrotation(x, filterVal, smoothedRotation); if(x<60) { x=90; //do //{ WRotation.write(x); delay(50); // }while(WRotation.read()>60); // Serial.println(x); } if (x>100) { x=170; // do //{ WRotation.write(x); delay(50); //}while(WRotation.read()<170); // Serial.println(x); }else { WRotation.write(x); } z = map(z, -1023, 1023, 0, 179); //Serial.print("Z:"); if(z<50) { z=40; } if (z>110) { z=100; } //Serial.print(z); WLeftRight.write(z); y = map(y, -1023, 1023, 40, 100); // Serial.print(" Y:"); // if(y<20) //{ // y=20; //} //if (y>110) //{ // y=120; //} // Serial.print(180 - y); WUpDown.write(180 - y); val = analogRead(0); // reads the value of the potentiometer (value between 0 and 1023) val = map(val, 0, 1023, 0, 179); val1 = analogRead(1); // reads the value of the potentiometer (value between 0 and 1023) val1 = map(val1, 0, 1023, 0, 179); val2 = analogRead(2); // reads the value of the potentiometer (value between 0 and 1023) val2 = map(val2, 0, 1023, 0, 179); val3 = analogRead(3); // reads the value of the potentiometer (value between 0 and 1023) val3 = map(val3, 0, 1023, 0, 179); val4 = analogRead(4); // reads the value of the potentiometer (value between 0 and 1023) val4 = map(val4, 0, 1023, 0, 179); filterVal = 0.05; smoothedVal = smooth(val, filterVal, smoothedVal); // second parameter determines smoothness - 0 is off, .9999 is max smooth smoothedVal1 = smooth(val1, filterVal, smoothedVal1); // second parameter determines smoothness - 0 is off, .9999 is max smooth smoothedVal2 = smooth(val2, filterVal, smoothedVal2); // second parameter determines smoothness - 0 is off, .9999 is max smooth smoothedVal3 = smooth(val3, filterVal, smoothedVal3); // second parameter determines smoothness - 0 is off, .9999 is max smooth smoothedVal4 = smooth(val4, filterVal, smoothedVal4); // second parameter determines smoothness - 0 is off, .9999 is max smooth if (smoothedVal > 120) { Pinky.write(60); // Serial.print("smoothed val: "); // Serial.print(smoothedVal); // Serial.print(" "); // Serial.print(val); // Serial.print(" "); // Serial.println("60"); } else if(smoothedVal<60) { Pinky.write(180); // Serial.print("smoothed val: "); // Serial.print(smoothedVal); // Serial.print(" "); // Serial.print(val); // Serial.print(" "); // Serial.println("180"); } delay(15); if (smoothedVal1 > 120) { Ring.write(60); } else if(smoothedVal1<60) { Ring.write(180); } delay(15); if (smoothedVal2 > 120) { Index.write(60); } else if(smoothedVal2<60) { Index.write(180); } delay(15); if (smoothedVal3 > 120) { Pointer.write(60); } else if(smoothedVal3<60) { Pointer.write(180); } delay(15); if (smoothedVal4 > 120) { Thumb.write(60); } else if(smoothedVal4<60) { Thumb.write(180); } delay(15); } void getGyroValues(){ byte xMSB = readRegister(L3G4200D_Address, 0x29); byte xLSB = readRegister(L3G4200D_Address, 0x28); x = ((xMSB << 8) | xLSB); byte yMSB = readRegister(L3G4200D_Address, 0x2B); byte yLSB = readRegister(L3G4200D_Address, 0x2A); y = ((yMSB << 8) | yLSB); byte zMSB = readRegister(L3G4200D_Address, 0x2D); byte zLSB = readRegister(L3G4200D_Address, 0x2C); z = ((zMSB << 8) | zLSB); } int setupL3G4200D(int scale){ //From Jim Lindblom of Sparkfun's code // Enable x, y, z and turn off power down: writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111); // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2: //writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000); // Configure CTRL_REG3 to generate data ready interrupt on INT2 // No interrupts used on INT1, if you'd like to configure INT1 // or INT2 otherwise, consult the datasheet: //writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000); // CTRL_REG4 controls the full-scale range, among other things: if(scale == 250){ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000); }else if(scale == 500){ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000); }else{ writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000); } // CTRL_REG5 controls high-pass filtering of outputs, use it // if you'd like: writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000); } void writeRegister(int deviceAddress, byte address, byte val) { Wire.beginTransmission(deviceAddress); // start transmission to device Wire.write(address); // send register address Wire.write(val); // send value to write Wire.endTransmission(); // end transmission } int readRegister(int deviceAddress, byte address){ int v; Wire.beginTransmission(deviceAddress); Wire.write(address); // register to read Wire.endTransmission(); Wire.requestFrom(deviceAddress, 1); // read a byte while(!Wire.available()) { // waiting } v = Wire.read(); return v; } int smooth(int data, float filterVal, float smoothedVal){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedVal = (data * (1 - filterVal)) + (smoothedVal * filterVal); return (int)smoothedVal; } int smooth1(int data, float filterVal, float smoothedVal1){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedVal1 = (data * (1 - filterVal)) + (smoothedVal1 * filterVal); return (int)smoothedVal1; } int smooth2(int data, float filterVal, float smoothedVal2){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedVal2 = (data * (1 - filterVal)) + (smoothedVal2 * filterVal); return (int)smoothedVal2; } int smooth3(int data, float filterVal, float smoothedVal3){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedVal3 = (data * (1 - filterVal)) + (smoothedVal3 * filterVal); return (int)smoothedVal3; } int smooth4(int data, float filterVal, float smoothedVal4){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedVal4 = (data * (1 - filterVal)) + (smoothedVal4 * filterVal); return (int)smoothedVal4; } int smoothrotation(int data, float filterVal, float smoothedRotation){ if (filterVal > 1){ // check to make sure param's are within range filterVal = .99; } else if (filterVal <= 0){ filterVal = 0; } smoothedRotation = (data * (1 - filterVal)) + (smoothedRotation * filterVal); return (int)smoothedRotation; }