//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.