RangeFinder.cpp

00001 
00002 #include <WProgram.h>
00003 #include <AFMotor/AFMotor.h>
00004 
00005 AF_Stepper motor(48, 2);
00006 
00007 /*
00008    Output pins from the GP2D02
00009    1) Ground (black)
00010    2) Input (Green) --> A1 (pClk)
00011    3) +5V (Red)
00012    4) Output (Yellow) --> A0 (pDat)
00013 
00014 */
00015 
00016 int pDat = 14; //A0
00017 int pClk = 15; //A1
00018 
00019 int numSteps = 0;
00020 
00021 void setupSensor()
00022 {
00023   pinMode(pClk, OUTPUT);
00024   pinMode(pDat, INPUT);
00025   digitalWrite(pClk, LOW);
00026 }
00027 
00028 byte readSensor()
00029 {
00030   byte dataIn = 0;
00031   digitalWrite(pClk, LOW);
00032   while(digitalRead(pDat) != HIGH);
00033 
00034   digitalWrite(pClk, HIGH);
00035   for(int i=0; i<8; i++)
00036   {
00037     digitalWrite(pClk, LOW);
00038     dataIn = (dataIn << 1) | digitalRead(pDat);
00039     digitalWrite(pClk, HIGH);
00040 
00041   }
00042   delay(2);
00043   digitalWrite(pClk, LOW);
00044   return dataIn;
00045 }
00046 
00047 void setupMotor()
00048 {
00049   motor.setSpeed(10);  // 10 rpm
00050   motor.release();
00051   delay(1000);
00052 }
00053 
00054 void setup() {
00055   beginSerial(19200);
00056   setupSensor();
00057   setupMotor();
00058   Serial.println("Setup Done");
00059 }
00060 
00061 void printDist(byte i, byte dist)
00062 {
00063   Serial.print((byte)255);
00064   Serial.print((byte)0);
00065   Serial.print((byte)255);
00066   Serial.print((byte)i);
00067   Serial.print((byte)dist);
00068 }
00069 
00070 
00071 
00072 void loop() {
00073 
00074   int nStep = 10;
00075   int scan = 200;
00076 
00077   for(int i=0; i<20; i++)
00078   {
00079     byte dist = readSensor();
00080     printDist(i,dist);
00081     motor.step(10, FORWARD, SINGLE);
00082   }
00083 
00084   for(int i=20; i>0; i--)
00085   {
00086     motor.step(10, BACKWARD, SINGLE);
00087     byte dist = readSensor();
00088     printDist(i,dist);
00089   }
00090 
00091 }
00092 
00093 int main()
00094 {
00095   init();
00096   setup();
00097 
00098   for(;;)
00099     loop();
00100 
00101   return 0;
00102 }
00103 
00104 
Generated on Sun May 8 08:41:31 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3