odometry

#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;
volatile uint8_t PINB_old=0;




//initialises Odometrie (Interrupts)
void initOdom(){
  DDRB = 0;
 //DDRB |= (1<<PB0)|(1<<PB3)|(1<<PB4)|(1<<PB7);

  PINB |= (1<<PB0)|(1<<PB3)|(1<<PB4)|(1<<PB7);


  PCMSK0 = 0;
  PCMSK0 |= (1<<PCINT0)|(1<<PCINT3)|(1<<PCINT4) | (1<<PCINT7);		//enable interrupts from ENC_1B and ENC_1A

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

  PINB_old = PINB;
  sei();
}

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

  cli();

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


      if((PINB & (1<<PB0)) > 0 ){
        if ((PINB_old & (1<<PB0)) != (PINB & (1<<PB0))){
          if ((PINB & (1<<PB3))){
            left = -10;
            //Serial.println("links rück");
          }
          else{
            left = 10;
            //Serial.println("links vor");
          }
        }}
  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
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
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

}