nextone

#include <avr/io.h>
#include <avr/interrupt.h>

#include <inttypes.h>

//Arduino.h has some Pi
#include <Arduino.h>
#include "Odometry.h"

volatile int right=0;		//=0:no movement;-10:backwarts; 10:forwards
volatile int left=0;			//=0:no movement;-10:backwarts; 10:forwards
int perimeter = PI*65/1000;
static volatile uint8_t PINB_old=0;





//initialises Odometrie (Interrupts)
void initOdom(){
  PCMSK0 = 0;
  PCMSK0 |= (1<<PCINT0) | (1<<PCINT3);		//enable interrupts from ENC_1B and ENC_1A

  PCICR =0;
  PCICR |= (1<<PCIE0);

  PINB_old = PINB;
}

ISR(PCINT0_vect){		//TODO: use of encoders has to be checked
  cli();


if ((PINB & (1<<PB0)) >0){
  //nur steigende flanke
 /* if ((PINB_old & (1<<PB0) & (1<<PB7)) != (PINB & (1<<PB0) & (1<<PB7))){
    right = -10;
    //rechts rückwärts
  }
  else  if ((PINB_old & (1<<PB0)) != (PINB & (1<<PB0))){
    right = 10;
   // rechts vorwärts
  }*/

    if ((PINB_old & (1<<PB0)) != (PINB & (1<<PB0))){
      if ((PINB_old & (1<<PB7)) != (PINB & (1<<PB7))){
         Serial.println("rechts rück");
    right = -10;}
      else{
         Serial.println("rechts vor");
        right = 10;}
  }
}

  if ((PINB & (1<<PB3)) >0){
    //nur steigende flanke
  /*if ((PINB_old & (1<<PB3) & (1<<PB4)) != (PINB & (1<<PB3) & (1<<PB4))){
    left = 10;
    Serial.println("links vor");
  }
  else  if ((PINB_old & (1<<PB3)) != (PINB & (1<<PB3))){
    left = -10;
    Serial.println("links rück");
  }*/

    if ((PINB_old & (1<<PB3)) != (PINB & (1<<PB3))){
      if ((PINB_old & (1<<PB4)) != (PINB & (1<<PB4))){
        Serial.println("links vor");
        left = 10;}

      else {
        left = -10;
         Serial.println("links rück");
      }
    }
  PINB_old = PINB;
  sei();
}
}

//writes count courrent of ticks to structure
void odomTicks(struct OdomData& d){
  cli();
  if (right == 10)
    d.right++;
  else if (right == -10)
    d.right--;
  if (left == 10)
    d.left++;
  else if (left == -10)
    d.left++;

  right = 0;
  left = 0;
  sei();
}

//m <- count
static float odomDistance( int32_t dticks)
{
  float dist = 0;

  if (dticks >0){				//moved forward
  while (dticks>224.2){
    dist += perimeter;
  }
  dist +=PI*(dticks*100/224.20);
}

  else{							//moved backward
   while (dticks<-224.2){
    dist -= perimeter;
  }
  dist +=PI*(dticks*100/224.20);
}
  return dist;
}

//m/s <- count, millis
//you may want to use parts of millis for short messurements
static float odomVelocity( int32_t dticks, float dtime)
{
  //v=s/t

  float distance = odomDistance(dticks);		//in m

 return distance/(dtime/1000);					//dtime/1000 because time is probably in ms

}