 PID rPID(&rIn, &rOut, &rSet, rkp, rki, rkd, DIRECT);
 
 os_timer_t myTimer;
+
+static inline int8_t sgn(int val) {
+ if (val < 0) return -1;
+ if (val==0) return 0;
+ return 1;
+}
 
 // start of timerCallback, repeat every "period"
 void tic(void *pArg) {

  Serial.print(lki);
  Serial.print(" lkd: ");
  Serial.println(lkd);
*/
  delay(1000);
}


extern "C" {
#include "user_interface.h"
}
#define DEBUG 1

double lpwm=0, rpwm=0;   // motor pwm
int lmc=0,  rmc=0;    // left motor counter
int lmc0=0, rmc0=0;   // last encoder value 
int ldir=1, rdir=1;   // motor direction
int llevel, rlevel;
double whesep= 0.135; // wheel separtion in m
double whedia= 0.7;   // wheel diameter in m
int CPR = 40;         // Encoder Count per Revolutions 
int period=50;        // PID sample timer period in ms
double lv=0., rv=0., lvt=0., rvt=0. ;// motor speed innumber tics per period with two methods
int ti=0;             // tic timer counter
double lIn,rIn,lOut,rOut,lSet=20,rSet=20;   // PID Input Signal, Output command and Setting speed for each wheel 
double lkp=0.5,lki=10,lkd=0.0;     // Left/right wheel PID constants. Can be modiffied while running with:
double rkp=0.5,rki=10,rkd=0.0;     // s nnn (setting point), p nnn (kp), i nnn (ki), d nnn (kd). nnn is divided by 10 to get decimals nnn -> nn.n
double kt=1000/period;            // number of periods/sec
int rcurrenttime, rlasttime, lcurrenttime, llasttime;

