
/*
  Barbot.cpp - Biblioth?que : Controle du robot Barbot.
  _________
  R?v : 1.00 
  Christophe Grosse 17/04/2018
  Le code est du domaine public.
*/
 
#include "Wire.h"
#include "TZ51_PAP1.h"
#include "TZ51_BARBOT.h"
#include <EEPROM.h>

#ifdef __AVR__
  #include <avr/pgmspace.h>
  #define WIRE Wire
#else
  #define PROGMEM
  #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
  #define WIRE Wire1
#endif
 
#if (ARDUINO >= 100)
  #include <Arduino.h> // capital A so it is error prone on case-sensitive filesystems
   // Macro to deal with the difference in I2C write functions from old and new Arduino versions.
  #define _I2C_WRITE write
  #define _I2C_READ read
#else
  #include <WProgram.h>
  #define _I2C_WRITE send
  #define _I2C_READ receive
#endif
 
Pap1 Stepper(23); 

Barbot::Barbot()
{
  step_per_dm=958.5;
 int vmax=253;
  step_per_degre=10.36;
}	

Barbot::Barbot(float n1,float a1,int v1)
{
 step_per_dm=n1;
 vmax=v1;
 step_per_degre=a1;
}	
 
unsigned int Barbot::calcule_angle(unsigned int angle)
{
 //convert an angle in degree into steps
 unsigned int temp=angle*step_per_degre;
 return temp;
}
unsigned long time_ir;

unsigned int Barbot::calcule_step(unsigned int dist,unsigned int unite)
{
 //convert a distance into steps
 unsigned int temp=dist*step_per_dm;
 switch (unite) {
  case 0 : temp=temp/100;
           break;
  case 1 : temp=temp/10;
           break;
 } // end switch
 return temp;
}

void Barbot::move(unsigned int dir,unsigned int distance,unsigned int unite,unsigned int pre)
{
 //Move forward or backward a given distance in mm or cm
 Stepper.SetMode(IDLE);
 unsigned int n=calcule_step(distance,unite);
 Stepper.StepA(n);
 Stepper.StepB(n);
 switch (dir) {
  case 0 : Stepper.DirA(CCW);
           Stepper.DirB(CW);
           break;
  case 1 : Stepper.DirA(CW);
           Stepper.DirB(CCW);
           break;
 } //end switch
 Stepper.SpeedA(vmax);
 Stepper.SpeedB(vmax);
 Stepper.SetMode(RELATIVE);
 delay(5);
 if (pre) {
   while (Stepper.Busy()) {}
    }
}

void Barbot::turn(unsigned int dir,unsigned int angle,unsigned int pre)
{
 //Turn right or left a given angle in degree
 Stepper.SetMode(IDLE);
 unsigned int n=calcule_angle(angle);
 Stepper.StepA(n);
 Stepper.StepB(n);
 switch (dir) {
  case 0 : Stepper.DirA(CCW);
           Stepper.DirB(CCW);
           break;
  case 1 : Stepper.DirA(CW);
           Stepper.DirB(CW);
           break;
 } //end switch
 Stepper.SpeedA(vmax);
 Stepper.SpeedB(vmax);
 Stepper.SetMode(RELATIVE);
 delay(5);
 if (pre) {
   while (Stepper.Busy()) {}
    }
}



void Barbot::stop()
{
 //Stop the bot
 Stepper.SetMode(IDLE);
 Stepper.StepA(0);
 Stepper.StepB(0);
 Stepper.SpeedA(0);
 Stepper.SpeedB(0);
} 


void Barbot::init()
{
  Wire.begin(); // join i2c bus
  Stepper.Begin(SINGLE_STEPPING,AUTO_RELAX);
  Stepper.SetMode(IDLE);
  Stepper.SpeedA(0);     //Motor A speed to 0
  Stepper.SpeedB(0);     //Motor B speed to 0
  Stepper.SetMode(CC);   //Enter in Continious Current Mode
  delay(500);
  Stepper.Begin(SINGLE_STEPPING,AUTO_RELAX);
  Stepper.SetMode(IDLE);
  Stepper.SpeedA(0);     //Motor A speed to 0
  Stepper.SpeedB(0);     //Motor B speed to 0
  Stepper.SetMode(CC);   //Enter in Continious Current Mode
  
}

void Barbot::IR38Write() {
 // 38Khz Burst on the tree IR LED
 // Launch this all 50ms at the maximum rate
 // 20 mesure per second maximum
 if ((millis()-time_ir)<50) {
    //Il faut encore attendre avant de refaire une mesure
    return;
   }
 time_ir=millis(); //On réinitialise le compteur
 for(int i = 0; i <=6; i++) {
  digitalWrite(5, LOW);
  delayMicroseconds(11);
  digitalWrite(5, HIGH);
  delayMicroseconds(11);
 }
 //Sauve le resultat des lectures
 IR_left=digitalRead(8);
 IR_center=digitalRead(A1);
 IR_right=digitalRead(9);
}

void Barbot::begin(uint32_t baud){
  serial->begin(baud);
}

String Barbot::floatToString(float valeur,int precision){
  char charVal[10];
  String stringVal = "";
  float valc=valeur*pow(0.1,precision);
  for(int i=0;i<sizeof(charVal);i++)
    {
     charVal[i]=' ';
    }
  dtostrf(valc, 0, precision, charVal);
  //convert chararray to string
  for(int i=0;i<sizeof(charVal);i++)
    {
     if (charVal[i]!=' ') {stringVal+=charVal[i];}
    }
  //supprime le dernier caractère de strinVal
  stringVal=stringVal.substring(0,stringVal.length()-1);
  return stringVal;
}

void Barbot::change_RamStorage_size(int taille){
  RamStorage=new double[taille];
  if(RamStorage!=NULL){
    RamStorage_size=taille;
  }
}

void Barbot::SaveEEPROM(int slot)
{
  int offset=(slot-1)*RamStorage_size*4;
  for(int i=0;i<RamStorage_size;i++){
  floatAsBytes.fval =  *(RamStorage+i); 
  EEPROM.write(offset+(i*2), highByte(floatAsBytes.bval[0]));
  EEPROM.write(offset+(i*2)+1, lowByte(floatAsBytes.bval[1]));
  EEPROM.write(offset+(i*2)+2, highByte(floatAsBytes.bval[2]));
  EEPROM.write(offset+(i*2)+3, lowByte(floatAsBytes.bval[3]));
  }
}

void Barbot::RestaureEEPROM(int slot)
{
  int offset=(slot-1)*RamStorage_size*4;
  for(int i=0;i<RamStorage_size;i++){
  *(RamStorage+i) = EEPROM.read(offset+(i*2))*256+ EEPROM.read(offset+(i*2)+1)+ EEPROM.read(offset+(i*2)+2)+ EEPROM.read(offset+(i*2)+3);
  }
}


void Barbot::playTone(int freq, int duration) {
 int tone=500000/freq;
 for (long i = 0; i < duration * 1000L; i += tone * 2) {
 digitalWrite(3, HIGH);
 delayMicroseconds(tone);
 digitalWrite(3, LOW);
 delayMicroseconds(tone);
 }
}