/* BAT Hat Blind Assistance Technology v01 : 2011-10-30 v02 : 2012-02-26 - Applying correct voltage to motors. v03 : 2012-03-03 - adjusting voltage for motors with different weights. Eric Pavey - www.akeric.com Much of the ping code used from: http://www.arduino.cc/en/Tutorial/Ping motor code pulled from my motorTest03 example. */ // all PMW digital pins const byte motorF = 9; const byte motorL = 10; const byte motorR = 6; // These arrays track which motors have big and small weights on them: const byte bigWeightMotors[] = { motorF}; const byte smlWeightMotors[] = { motorL, motorR}; const byte pingF = 8; const byte pingL = 7; const byte pingR = 11; // used for debug printing: const byte debugMotor = pingF; // Change this if using a 3.3v Arduino, for example. const float arduinoVoltage = 5.0; // Motors are from xbox 360 controller: 2v max, pull less than 20mA // The voltage where the motor will start spinning. Voltage less than this probably // won't make it spin. const float bigWeight_minMotorVoltage = 1.75; const float bigWeight_maxMotorVoltage = 2.5; const float smlWeight_minMotorVoltage = 1.25; const float smlWeight_maxMotorVoltage = 2.0; // Max is the range in cm where the motors will start spinning at low speed, // min is the distance where the motors are spinning at max speed. const float maxDistCm = 150.0; const float minDistCm = 100.0; // Updated in setup(), these controll the values written to the motors from the Arduino. int bigWeight_minSpeed = 0; int bigWeight_maxSpeed = 0; int smlWeight_minSpeed = 0; int smlWeight_maxSpeed = 0; void setup() { // initialize serial communication: Serial.begin(9600); pinMode(motorL, OUTPUT); pinMode(motorF, OUTPUT); pinMode(motorR, OUTPUT); // pinMode for the ping pins is switched in the pinger function. // based on our voltages, turn them into values to apply to analogWrite(): bigWeight_minSpeed = int((bigWeight_minMotorVoltage / arduinoVoltage) * 255.0); bigWeight_maxSpeed = int((bigWeight_maxMotorVoltage / arduinoVoltage) * 255.0); smlWeight_minSpeed = int((smlWeight_minMotorVoltage / arduinoVoltage) * 255.0); smlWeight_maxSpeed = int((smlWeight_maxMotorVoltage / arduinoVoltage) * 255.0); } void loop(){ pinger(pingL, motorL); delay(100); pinger(pingF, motorF); delay(100); pinger(pingR, motorR); delay(100); } // This function does all the work: It both gathers ping info, and turns it into // a motor speed. void pinger(byte pingPin, byte motorPin) { // establish variables for duration of the ping, // and the distance result in inches and centimeters: long duration, inches, cm, feet; // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: pinMode(pingPin, OUTPUT); digitalWrite(pingPin, LOW); delayMicroseconds(2); digitalWrite(pingPin, HIGH); delayMicroseconds(5); digitalWrite(pingPin, LOW); // The same pin is used to read the signal from the PING))): a HIGH // pulse whose duration is the time (in microseconds) from the sending // of the ping to the reception of its echo off of an object. pinMode(pingPin, INPUT); duration = pulseIn(pingPin, HIGH); // Convert the time into a distance cm = microsecondsToCentimeters(duration); // now figure out how much to spin the motor: int motorSpeed = 0; if(cm < maxDistCm){ //float motorVal = 255.0 - map(float(cm), 0.0, 400.0, 0.0, 255.0); if(itemInArray(motorPin, bigWeightMotors) == true){ motorSpeed = map(cm, maxDistCm, minDistCm, bigWeight_minSpeed, bigWeight_maxSpeed); Serial.print(motorPin); Serial.println(" big motor"); } else if(itemInArray(motorPin, smlWeightMotors) == true){ motorSpeed = map(cm, maxDistCm, minDistCm, smlWeight_minSpeed, smlWeight_maxSpeed); Serial.print(motorPin); Serial.println(" small motor"); } } // just some debug printing: if(pingPin == debugMotor){ Serial.print(cm); Serial.print("cm, "); Serial.print(motorSpeed); Serial.print(" motorSpeed, "); if(cm < maxDistCm){ Serial.print(" Running!"); } else{ Serial.print(" Stopped!"); } Serial.println(); } analogWrite(motorPin, motorSpeed); } // Since I don't know C that good, this was the best I could come up with to // figure out if an item was in an array. boolean itemInArray(const byte item, const byte array[]){ boolean ret = false; int arraySize = sizeof(array) / sizeof(array[0]); for(byte i=0; i