Servicio Social / Enero-Mayo 2012 (V4)

//ITESM-Campus Estado de Mexico //Servicio Social / Enero-Mayo 2012 //Andres Tosca Ramirez //A01062850 //Codigo para el control de la silla de ruedas con acelerometro //Version 4.0 #include"MMA8452.h" // Set the scale below either 2, 4 or 8 const byte SCALE = 2; // Sets full-scale range to +/-2, 4, or 8g. Used to calc real g values. // Set the output data rate below. Value should be between 0 and 7 const byte dataRate = 0; // 0=800Hz, 1=400, 2=200, 3=100, 4=50, 5=12.5, 6=6.25, 7=1.56 // Pin definitions int int1Pin = 2; // These can be changed, 2 and 3 are the Arduinos ext int pins int int2Pin = 3; float e = -2.0; int accelCount[3]; // Stores the 12-bit signed value float accelG[3]; // Stores the real accel value in g's float accelG1[3]; //Stores the initial values //Flags for locking and unlocking the motors of the wheelchair int flagLeft = 0; int flagLeft2 = 0; int flagRight = 0; int flagBlocked = 0; int flagLeft3 = 0; int flagRight2 = 0; int flagRight3 = 0; int firstLocked = 0; int firstUnlocked = 0; int i = 0; //Outputs for the relays int outputFirst = 5; int outputSecond = 6; int outputThird = 7; int outputFourth = 8; int motorPin = 11; // L293D Pin En1 connected to pin PWM 11 int firstLecture = 0; void setup(){ pinMode(motorPin, OUTPUT); //It is specified pins are going to behave as output pinMode(outputFirst, OUTPUT); pinMode(outputSecond, OUTPUT); pinMode(outputThird, OUTPUT); pinMode(outputFourth, OUTPUT); byte c; //This is the baud for the terminal in order to obtain the debug info Serial.begin(57600); // Set up the interrupt pins, they're set as active high, push-pull pinMode(int1Pin, INPUT); digitalWrite(int1Pin, LOW); pinMode(int2Pin, INPUT); digitalWrite(int2Pin, LOW); // Read the WHO_AM_I register, this is a good test of communication c = readRegister(0x0D); // Read WHO_AM_I register if (c == 0x2A){ // WHO_AM_I should always be 0x2A initMMA8452(SCALE, dataRate); // init the accelerometer if communication is OK Serial.println("MMA8452Q is online..."); }else{ Serial.print("Could not connect to MMA8452Q: 0x"); Serial.println(c, HEX); while(1) ; // Loop forever if communication doesn't happen } } void loop(){ static byte source; // If int1 goes high, all data registers have new data if (readRegister(STATUS)&0x04){ // Interrupt pin, should probably attach to interrupt function readAccelData(accelCount); // Read the x/y/z adc values for (int i=0; i<3; i++) accelG[i] = (float) accelCount[i]/((1<<12)/(2*SCALE)); // get actual g value, this depends on scale being set // Print out values //Serial.print(accelG[2]*9.81,4); // Print g values for (int i=0; i<3; i++){ //Serial.print(accelG[i]*9.81, 4); // Print g values //Serial.print("\t\t"); // tabs in between axes } if(firstLecture == 0){ accelG1[0] = accelG[0]; accelG1[1] = accelG[1]; accelG1[2] = accelG[2]; firstLecture++; } //The sequence for locking the motors of the wheelchair is: Left-Right-Left //The sequence for unlocking the motors of the wheelchair is: Rigth-Left-Right //The indicator when the wheelchair is locked or unlocked is by a vibration //This If corresponds for the LEFT interpretation if(accelG[1]*9.81 <= ((accelG1[1]*9.81)-1.22)){ Serial.print("LEFT"); flagLeft ++; if(flagRight >=3 ){ flagRight = 0; } if(flagRight2 >=3 ){ flagRight2 = 0; } if(flagRight3 >=3 ){ flagRight3 = 0; } if(flagBlocked == 1){ flagLeft2 ++; flagLeft = 0; } if(flagLeft2 < 3 && flagLeft2 > 0 && flagBlocked == 1){ flagBlocked = 2; } if(flagBlocked == 2){ flagLeft3 ++; } if(flagLeft3 >= 3){ flagLeft3 = 0; } if(flagRight2 > 0 && flagRight2 < 3 && flagLeft3 > 0 && flagLeft3 < 3 && flagBlocked == 2){ flagBlocked = 3; } if(flagBlocked == 2 || flagBlocked == 3){ Serial.println(" BLOCKED"); firstUnlocked = 0; firstLocked++; i = 0; if(firstLocked == 1){ while(i < 150){ analogWrite(motorPin, 170); // turn ON the Motor Serial.println("Vibrando"); i++; } analogWrite(motorPin, 0); } }else{ Serial.println(""); digitalWrite(outputFirst, HIGH); digitalWrite(outputSecond, LOW); digitalWrite(outputThird, LOW); digitalWrite(outputFourth, HIGH); } //This if corresponds to the RIGTH interpretation 2.0 }else if(accelG[1]*9.81 >= ((accelG1[1]*9.81)+2.22)){ Serial.print("RIGHT"); flagRight ++; if(flagLeft >= 3 ){ flagLeft = 0; } if(flagLeft2 >= 3 ){ flagLeft2 = 0; } if(flagLeft3 >= 3 ){ flagLeft3 = 0; } if(flagLeft < 3 && flagLeft > 0 && flagRight < 3 && flagRight > 0 && flagBlocked == 0){ // Necesito el regreso flagBlocked = 1; } if(flagBlocked == 2){ flagRight2 ++; } if(flagBlocked == 3){ flagRight3 ++; } if(flagRight3 < 3 && flagRight3 > 0 && flagBlocked == 3){ flagBlocked = 0; firstLocked = 0; firstUnlocked++; if(firstUnlocked == 1){ i = 0; while(i < 250){ analogWrite(motorPin, 200); // turn ON the Motor if(i == 125){ analogWrite(motorPin, 0); delay(100); } analogWrite(motorPin, 200); // turn ON the Motor Serial.println("Vibrando2"); i++; } analogWrite(motorPin, 0); } } if(flagBlocked == 2 || flagBlocked == 3){ Serial.println("BLOCKED"); }else{ digitalWrite(outputFirst, LOW); digitalWrite(outputSecond, HIGH); digitalWrite(outputThird, HIGH); digitalWrite(outputFourth, LOW); Serial.println(""); } //This if corresponds to the FRONT interpretation -2.9 }else if(accelG[0]*9.81 < ((accelG1[0]*9.81)-3.06)){ Serial.print("FRONT"); if(flagLeft >= 3 ){ flagLeft = 0; } if(flagRight >=3 ){ flagRight = 0; } flagLeft = 0; flagLeft2 = 0; flagLeft3 = 0; flagRight = 0; flagRight2 = 0; flagRight3 = 0; if(flagBlocked != 2){ flagBlocked = 0; } if(flagBlocked == 2 || flagBlocked == 3){ Serial.println(" BLOCKED"); }else{ Serial.println(""); digitalWrite(outputFirst, HIGH); digitalWrite(outputSecond, LOW); digitalWrite(outputThird, HIGH); digitalWrite(outputFourth, LOW); } //This if corresponds to the BACK interpretation 1.3 }else if(accelG[0]*9.81 > ((accelG1[0]*9.81)+1.46)){ Serial.print("BACK"); if(flagLeft >= 3 ){ flagLeft = 0; } if(flagRight >=3 ){ flagRight = 0; } flagLeft = 0; flagLeft2 = 0; flagLeft3 = 0; flagRight = 0; flagRight2 = 0; flagRight3 = 0; if(flagBlocked != 2){ flagBlocked = 0; } if(flagBlocked == 2 || flagBlocked == 3){ Serial.println(" BLOCKED"); }else{ Serial.println(""); digitalWrite(outputFirst, LOW); digitalWrite(outputSecond, HIGH); digitalWrite(outputThird, LOW); digitalWrite(outputFourth, HIGH); } }else{ flagLeft = 0; flagLeft2 = 0; flagLeft3 = 0; flagRight = 0; flagRight2 = 0; flagRight3 = 0; digitalWrite(outputFirst, LOW); digitalWrite(outputSecond, LOW); digitalWrite(outputThird, LOW); digitalWrite(outputFourth, LOW); } } delay(1000); // Delay here for visibility }

Be the first to comment

You can use [html][/html], [css][/css], [php][/php] and more to embed the code. Urls are automatically hyperlinked. Line breaks and paragraphs are automatically generated.