Long Way Home

/* Auswertung von Winkelencoder mit Lochscheibe
/* und zwei Reflex-Lichtschranken
/*
 * Lab3 09/2012 Nawrath
 * Kunsthochschule fuer Medien Koeln
 * Academy of Media Arts Cologne
 * h ttp://interface.khm.de
 
 thanks to adafruit, ideas have be taken from the
 Arduino library for the  LPD8806-based strip.
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */




#include <AFMotor.h>

AF_DCMotor motor(1, MOTOR12_64KHZ);


int pinIRGnd=19;
int pinIRVcc=18;
int pinIRb=17;
int pinIRa=16;

int pinLed= 13; 
  

#define debug 0



/* SETUP */
void setup () {
  Serial.begin(115200);
  pinMode(pinLed, OUTPUT);


  pinMode(pinIRGnd, OUTPUT);
  pinMode(pinIRVcc, OUTPUT);
  pinMode(pinIRa, INPUT);
  pinMode(pinIRb, INPUT);
  digitalWrite(pinIRVcc,1);

}

/* MAIN LOOP */
//****************************************************************************
void loop(){

 int SpeedPoti = analogRead(A0);
 motor.setSpeed(SpeedPoti*0.249);

 if (SpeedPoti>10)
 {
   motor.run(FORWARD);
 }
 else
 {
   motor.run(RELEASE);
 }


  int ww=getIrSensors();

}
//****************************************************************************
int getIrSensors() {

  int teilungen=24;   // Anzahl der Sektoren auf der Scheibe
  int nMax= 1;       // max Umdregungen
  
  int teilgrad=360/teilungen;

  static int  iraa,irba,winkel;
  static long milA,milAa,milAaT;
  static int f_init=1;
  static int tSektor,tSektorTeil;
  static int wTeilSektor;
  static int cntSektor,cnt360;



  int ira= digitalRead(pinIRa);  // Sektor Spur
  int irb= digitalRead(pinIRb);  // 360 Spur
  digitalWrite(pinLed,ira);


  milA=millis();

  if (milA >= milAaT + tSektorTeil){   // Winelausgabe berecnet aus Sektore  x Umdehungen + extrapoliertem Winke
    wTeilSektor++;
    winkel=wTeilSektor+cntSektor*360/teilungen+cnt360*360;

    milAaT=milA;
    
       Serial.println(winkel );
     
  }


  if (ira == 1 && iraa ==0 ) {   // Sektor Spur
#if debug 
    Serial.print("*** Sektor ");
#endif
    //  if (!f_360 )
    cntSektor++;
    //  f_360=0;

    tSektor= milA-milAa;
    tSektorTeil= tSektor/teilgrad;
    milAa=milA;
    wTeilSektor=0;
    winkel=wTeilSektor+cntSektor*360/teilungen+cnt360*360;
#if debug     
    Serial.print("sec:");
    Serial.print(cntSektor);

    Serial.print(" 360:");
    Serial.print(cnt360);
    Serial.print(" tS:");
    Serial.print(tSektor);
    Serial.print(" tW:");
    Serial.print(tSektorTeil);
        Serial.print(" W:");
    Serial.print(winkel);
    Serial.println("");
#endif

  }


  if (irb == 1  && irba == 0) {  // 360 Spur
#if debug 
    Serial.print("### 360 ");
#endif
    while ( digitalRead(pinIRa)==0) {
      delay(1);

    }
    wTeilSektor=0;
    cnt360++;
    cntSektor=0;
    ira=1;
    if (f_init) {
      cnt360=0;
      f_init=0;
    }
    if (cnt360 >= nMax ) cnt360=0;
  }


  iraa=ira;
  irba=irb;

  return(winkel);

}