YAAR - Yet Another Arduino Robot

// Rev4 pwb, 60 mA, Mega2560 Pro

// 2021_0905 all works, changed piPwrOff()
// 0911 opcom, picom moved to 10mS, distance calc moved to isrs, speedup->3x
// 0912 check conditions, state mgr moved to 10 mS, speedup->2x, 170k x/s, tested PIR @ 1mS but doubles exe time
// 0913 servo steering
// 1001 1mS PIR timebase
// 1002 piComFailFlag
// 1003 10 mS PIR timebase
// 2022_0004 Rev 5 PWB, added quiet mode
// 0101 revert to DRV8833 direct attach, separate TB6612
// 0103 pi pwr control revision
// 0104 drive()->speedCtrl() with changes to handle Kp in reverse
// 0105 timer2 pwm invert bit for DRV8833
// 0106 experiment with i-term in speed control -> no improvement, vref = 1.1v
// 0107 clean up variables
// 0108 eliminated dirA,B in DRV8833 code
// 0200 LAZ to OC3A (5), servo to OC4C
// 0201 bug in rotate wont stop, kick on start
// 0204 fixed steering residual error, DRV8833 rewrite
// 0205 added adjust servo center in op interface, changed init sequence to avoid servo slew to max
// 0207 servoCenter store to eeprom
// 0209 added halt, brake
// 0210 3 quiet states
// 0211 menu add Kill power
// 0222 remove speed arg from F,R
// 0224 bug in spin
// 0225 reorganize steer()
// 0226 remove caution state, rewrite obstacle detect, store quiet in EEPROM, activated low bat test
// 0301 fwdZone more speed compensation
// 0305 add all commands to Pi menu
// 0306 add pushbutton release required to start
// 0307 pi off if no battery
// 0309 DRV8833 simplified
// 0310 Kp->eeprom
// 0311 TB6612
// 0320 back to DRV8833
// 0321 ground bug finally identified and fixed!!!
// 0322 changed variable names associated with motor contoller


//                                     +--------------------------------------- +
//                                VIN  |O O                                     |
//                                GND  |O O                                     |
//                                5V   |O O                                     |
//                                3V3  |O O                                     |
//                             RST     |O O     AREF                            |
//                 TX0         PE1 1   |O O  0  PE0 RX0               PF1 A1 O O| A0  PF0           EN_PIPWR
//           OC3C INT5         PE5 3   |O O  2  PE4 OC3B INT4         PF3 A3 0 0| A2  PF2
// laser     OC3A              PE3 5   |O O  4  PG5 OC0B              PF5 A5 O O| A4  PF4         
// SERVO     OC4B              PH4 7   |O O  6  PH3 OC4A              PF7 A7 O O| A6  PF6
// PWMB      OC2B              PH6 9   |O O  8  PH5 OC4C         PCINT17  A9 O O| A8  PK0 PCINT16   BUMPA | BUMPB
// BEEP      OC1A      PCINT5  PB5 11  |O O  10 PB4 OC2A PCINT4  PCINT19 A11 O O| A10 PK2 PCINT18   pushbutton
//           OC0A OC1C PCINT7  PB7 13  |O O  12 PB6 OC1B PCINT6  PCINT21 A13 O O| A12 PK4 PCINT20
//           RX3       PCINT9  PJ0 15  |O O  14 PJ1 TX3  PCINT10 PCINT23 A15 O O| A14 PK6 PCINT22
//           RX2               PH0 17  |O O  16 PH1 TX2               PC4 33 O O| 32  PC5
//           RX1  INT2         PD2 19  |O O  18 PD3 TX1 INT3          PC2 35 O O| 34  PC3
//           SCL  INT0         PDO 21  |O O  20 PD1 SDA INT1          PC0 37 O O| 36  PC1
//                             PA1 23  |O O  22 PA0                   PG2 39 O O| 38  PD7 
//                             PA3 25  |O O  24 PA2                   PG0 41 O O| 40  PG1
//                             PA5 27  |O O  26 PA4                   PL6 43 O O| 42  PL7
//                             PC7 29  |O O  28 PA6                   PL4 45 O O| 44  PL5
//                             PC6 31  |O O  30 PC7                   PL2 47 O O| 46  PL3           
//                                     +----------------------------------------+
//                                          PL0 49 O O 48 PL1                                       
//                                  PCINT2  PB2 51 O O 50 PB3 PCINT3
//                                  PCINT0  PB0 53 O O 52 PB1 PCINT1    EN5V
#include <EEPROM.h>

// **************************************************************************************
// *************************** Constants and Variables **********************************
// **************************************************************************************

//------------------------ pin assignments -------------------------------
int const tx2pin          = 16;   // serial comm to Pi
int const rx2pin          = 17;   // serial comm to Pi
int const echoPin         = 2;    // SR04 Int4, echo
int const trigPin         = 4;    // SR04 trigger
int const lazPin          = 5;    // laser active low
int const servo1pin       = 6;    // servo pin, OC4A
int const servo2pin       = 7;    // servo pin, OC4B
int const servo3pin       = 8;    // servo pin, OC4C
int const pwmApin         = 9;    // motor PWM, left,  OC2A
int const pwmBpin         = 10;   // motor PWM, right, OC2B
int const encoder1pin     = 18;   // INT2
int const encoder2pin     = 19;   // INT3
int const vBatPin         = A3;   // battery monitor
int const bump1pin        = 62;   // PCINT16
int const bump2pin        = 63;   // PCINT17
int const pbPin           = 64;   // PCINT18, pushbutton
int const en5vPin         = 52;   // enable 5V power to this board
int const enPiPwrPin      = 54;   // enable 5V power to Pi
int const piOnPin         = 55;   // signal to Pi to shutdown
int const led1pin         = 56;   // LED 1 active high
int const led2pin         = 59;   // LED 2 active high
int const beepPin         = 11;   // beep, OC1A
int const beepGndPin      = 13;   // beep ground, OC1C
int const scopePin        = 29;   // debug

// ------------------- Motor Control Pins DRV8833/TB6612 --------------
// specific to motor control board
int const enM12pin        = 47;   // H-bridge control
int const dirM1Apin       = 48;   // H-bridge control
int const dirM2Apin       = 49;   // H-bridge control
int const dirM1Bpin       = 45;   // not used DRV8833
int const dirM2Bpin       = 46;   // not used DRV8833

// -------------------------  Global constants ---------------------
float const  bitsToVolts  = .012; // calibrated on PwrDistV5, 1M/100k
float const  batLow       = 6.3;  // low battery threshold, tested, use 6.3
float const  wheelDia     = 6.94; // wheel dia in cm
float const  trackWidth   = 16.0; // track width in cm
int   const  holes        = 50;   // number of slots in optical wheel encoder
int   const servoMin      = 800;  // .5uS/tic, less will cause servo to stall, avoid, ~3000=center
int   const servoMax      = 4500; // more will cause servo to stall -> high current, avoid
int   const ticsPerDeg    = 19;   // servo steering, number of timer ticks / deg of servo rotation
int   const speedMaxFwd   = 99;   // set to limit max forward speed
int   const speedMaxRev   = 25;   // set to limit max reverse speed, not used 
int   const  rate         = 20;   // execution rate in ms as set in tasker, used to compute deltaT
byte  const  STOP         = 0;    // used by mtrDirL and mtrDirR
byte  const  FWD          = 1;    // constants for code readability
byte  const  REV          = 2;
byte  const porState      = 0;    // Power On Reset entry point
byte  const stopState     = 1;    // stopped
byte  const fwdState      = 2;    // going forward
byte  const revState      = 3;    // going backward
byte  const spinState     = 4;    // rotating
boolean const servoDir    = 0;    // change 0 or 1 to reverse steering servo direction
boolean const mtrAonLeft  = false;// motor A is on left side, =false if on right

// ------------------------------- Variables ---------------------------------------------
byte  state;                    // state machine state
float batVolts;                 // battery voltage, nominally 7.4V, 2 cell LiPo, do not exceed 10.8V
float cmPerTick;                // number of mm traveled per encoder tick
float degPerTick;               // angle traversed if one wheel stopped
float degPerCm;                 // angle produced by delta left/right distance
float deltaT;                   // execution rate of speed calc, factor = execution rate x .3
long unsigned loops;            // show main loops/second execution rate
int unsigned seconds;           // seconds counter, very long time
int unsigned zoneFwd;           // forward obstacle avoidance zone, =f(speed)
int servoCenter;                // ~3000 center, saved to eeprom during adjustment (menu "A")
boolean collisionFlag;          // flag set if bumper switch activated
boolean obstacleFlag;           // flag set if ultrasonic detects obstacle ahead
boolean distanceFlag;           // flag set if distance command has been achived
boolean angleFlag;              // flag set if desired angle achieved
boolean batLowFlag;             // flag set if bat volts less than bat low threshold
boolean opCmdFlag;              // flag set if stop command came from operator
boolean piCmdFlag;              // flag set if stop command came from Pi
boolean piComFailFlag;          // flag set if communicatioons failure between Pi and mega2560
boolean beepFlag;               // flag set if beeper is beeping
boolean beepOnOffFlag;          // beep or silence between beeps
boolean pushbuttonFlag;         // flag set if pushbutton is pressed
boolean batPwr;                 // battery connected - pi can be powered up
boolean volatile newPingFlag;   // flag set if new ping data available
byte    quietState;             // beep can be quiet, soft or loud
byte    beepNumber;             // number of beeps

// --------------------------- Speed / distance ------------------------------------
long distanceCmd;               // desired distance in cm
long distanceAct;               // actual distance traveled
long distanceSave;              // used by pause state to restore original distance cmd
int  speedCmd;                  // desired speed
int  speedAct;                  // actual speed (L+R/2)
int  speedSave;                 // used by pause state to restore speed cmd
int  speedCmdA;                 // desired speed left motor ("1")
int  speedCmdB;                 // right motor
int  speedActA;                 // actual speed left motor  ("2")
int  speedActB;                 // right motor
float  trackCmd;                // must be float to avoid residual error in servo steering
float  trackAct;                // rotation achieved
float  trackErr;                // rotation remaining
int  bearingCmd;                // bearing is in degrees
int  bearingErr;                // cmd - act
int  bearingAct;                // bearing is track difference
float angle;                    // bearing, used by steering()
float volatile motorDistB;      // distance is stored in cm
float volatile motorDistA;      // track
float lastMotorDistB;           // used to calculate speed
float lastMotorDistA;           // speed
int  pathRadius;                // radius of path, cm
int  pwmA;                      // pwm to motor A
int  pwmB;                      // pwm to motor B
int  mtrAside;                  // =1 if motorA is on left, =-1 if motorA is on right, calc at startup
byte Kp = 10;                   // speed correction gain
byte Ks = 6;                    // steering correction gain, use only even numbers - reverse /2 then *2

// ------------------------------ Ping ---------------------------------------------
unsigned int volatile startCount;// 4 uS/tick
int unsigned volatile pingDist; // ping distance in cm, must init so debugging w/o ping->no collide
int unsigned volatile pingStart;// Timer count
byte obstacleCount;

// ----------------------------- Terminal control -----------------------------------
int lineCounter;                // renew heading title line counter
byte report;                    // which report to show
const char    TAB       = 9;    // separator for reports
const char    CR        = 13;   // DEC ternminal control (Putty)
const char    LF        = 10;
const char    ESC       = 27;

// ***********************************************************************************************
// ***************************** Start Code ******************************************************
// ***********************************************************************************************

// -----------------------------------------------------------------------------------------------
// -----------------------------  Interrupt ISRs -------------------------------------------------
// ----------------------------- External Ints 2 & 3, Slot encoders ------------------------------
ISR(INT2_vect) {                          // which is left/right is a wiring issue
  motorDistA += cmPerTick;                // distance traveled
  distanceAct = (motorDistB+motorDistA)/2;// (L+R)/2, in cm
}
ISR(INT3_vect) {                          // interchange int2&3 at will
  motorDistB += cmPerTick;                // at 100 cm/sec -> 500 tics/sec
  distanceAct = (motorDistB+motorDistA)/2;// it is faster to do dist calc here
}

// ---------------------------PCINT0, 0-7 pin change interrupt -------------------------------------
// PCINT2&3 are available but not used
ISR(PCINT0_vect) {                        // 5 uS with digitalWrite(2uS)
}

// ---------------------------PCINT1, 8-15 pin change interrupt ------------------------------------
ISR(PCINT1_vect) {                        //
}

// ---------------------------PCINT2, 16-23 pin change interrupt -----------------------------------
ISR(PCINT2_vect) {                                  // bumpers, pushbutton
  if(!digitalRead(bump1pin)) collisionFlag  = true; // bumper has pullup, switch to gnd
  if(!digitalRead(bump2pin)) collisionFlag  = true;
  if(digitalRead(pbPin))     pushbuttonFlag = true; // pushbutton goes high when pressed
}

// ---------------------------- External Int 4, Ping ---------------------------------------------
ISR(INT4_vect) {
  if(digitalRead(echoPin)) startCount = TCNT3;      // get count on rising edge
  else {                                            // falling edge - calc results
    pingDist = (TCNT3-startCount)/15;               // x.068 cm/tick = /14.7 ticks/cm
    newPingFlag = true;                             // set flag to indicated new data
  }
}

// ---------------------------- External Int 5 -----------------------------------------------
ISR(INT5_vect) {            // available but not used
}

// --------------------------- WDT Periodic Interrupt --------------------------------------------
ISR(WDT_vect) {             // interrupt is called 16 x/sec, every 64 ms -> 8 Hz
  PINE = 8;                 // fast toggle OC3A, PE3 (port E, bit 3), tricky!
}

// --------------------------- Timer 2 overflow interrupt ----------------------------------------
ISR(TIMER2_OVF_vect) {  
}
// --------------------------- Timer 3 overflow interrupt ----------------------------------------
ISR(TIMER3_OVF_vect) {
}
// --------------------------- Timer 4 overflow interrupt ----------------------------------------
ISR(TIMER4_OVF_vect) {
}

// --------------------------- Timer 5 Periodic Interrupt ------------------------------------
int unsigned volatile task;// timer 5 set to interrupt 100x/sec, every 10 mS
boolean volatile xps100;   // sets 100x/sec
boolean volatile xps50;    // 50 x/s
boolean volatile xps20;    // 20 x/s
boolean volatile xps10;    // 10 x/s
boolean volatile xps05;    // 5 x/s
boolean volatile xps02;    // 2 x/s
boolean volatile xps01;    // sets 1x/s
ISR(TIMER5_COMPA_vect) {   // interrupt is called 100x/sec
  task++;                  // 100 uS @ 1kHz rate or at 100 Hz rate
  xps01 = ((task%100)==0); // 1x/sec, sets every second
  xps02 = ((task%50)==0);  // 2x/sec
  xps05 = ((task%20)==0);  // 5x/sec
  xps10 = ((task%10)==0);  // 10x/sec
  xps20 = ((task%5)==0);   // 20x/sec
  xps50 = ((task%2)==0);   // 50x/sec
  xps100 = true;           // 100 x/s
}
  
//------------------------------------setup---------------------------------------
void initializeIO() {                     // 1 second bootloader delay to this point
  pinMode(encoder1pin,    INPUT_PULLUP);  // if disconnected, pullup stable
  pinMode(encoder2pin,    INPUT_PULLUP);  // if disconnected, pullup stable
  pinMode(bump1pin,       INPUT_PULLUP);  // required, no external pullup
  pinMode(bump2pin,       INPUT_PULLUP);  // required, no external pullup
  pinMode(echoPin,        INPUT_PULLUP);  // if disconnected, pullup stable
  pinMode(trigPin,        OUTPUT);        // SR04 trigger
  pinMode(pwmApin,        OUTPUT);        // TB6612 pwm input, motor A
  pinMode(pwmBpin,        OUTPUT);        // TB6612 pwm input, motor B
  pinMode(servo1pin,      OUTPUT);        // servo 1
  pinMode(servo2pin,      OUTPUT);        // servo 2
  pinMode(servo3pin,      OUTPUT);        // servo 3
  pinMode(led1pin,        OUTPUT);        // laser, active low
  pinMode(led2pin,        OUTPUT);        // status led, active high
  pinMode(lazPin,         OUTPUT);        // status led, active high
  pinMode(scopePin,       OUTPUT);        // diagnostic
  pinMode(en5vPin,        OUTPUT);        // turns on 5v to this board
  pinMode(enPiPwrPin,     OUTPUT);        // turns on 5v to Pi
  digitalWrite(enPiPwrPin,LOW);           // power off until turned on
  digitalWrite(en5vPin,   HIGH);          // enable 5V to this board, thru pushbutton until now
}

// ----------------------------Serial setup ----------------------------------------------
void initializeSerial() {
  Serial.begin(9600);                   // 9600 Baud to monitor
  Serial.setTimeout(500);               // timeout xxx ms (IDE monitor)
  delay(100);
  Serial.println();
}

void initializeBeep() {
  switch(quietState) {
    case 0:                             // silence
      pinMode(beepPin, INPUT);          // OFF
      pinMode(beepGndPin,INPUT);        // piezo beeper return
      Serial.print("Beep off");
      break;
    case 1:  
      pinMode(beepPin, OUTPUT);         // audio
      pinMode(beepGndPin,INPUT);        // discharges thru input protection diodes (not good)    
      Serial.print("Beep soft"); 
      break;
    case 2:
      pinMode(beepPin,OUTPUT);          // output full power
      pinMode(beepGndPin,OUTPUT);       // output
      digitalWrite(beepGndPin, LOW);    // piezo return
      Serial.print("Beep loud");
  }
  Serial.println();
}

void initializeTimers() {
// --------------------------- TIMER_0 8 bit, ------------------------------------
// timer0 is not used, used by delay()
// TCCR0A COM0A1  COM0A0  COM0B1  COM0B0  ----    ----    WGM01   WGM00
// TCCR0B FOC0A   FOC0B   ----    ----    WGM02   CS02    CS01    CS00
// TIMSK0 ----    ----    ----    ----    ----    OCIE0B  OCIE0A  TOIE0

// --------------------------- TIMER_1 16 bit, beep ------------------------------
// OC1C is piezo return (gnd), might be used for dual tone or double volume in pwm mode
// TCCRnA COMnA1  COMnA0  COMnB1  COMnB0  COMnC1  COMnC0  WGMn1   WGMn0
// TCCRnB ICNCn   ICNSn   ------  WGMn3   WGMn2   CSn2    CSn1    CSn0
// TIMSKn ----    ----    ICIEn   ----    OCIEnC  OCIEnB  OCIEnA  TOIEn
   TCCR1A = B01000000;  // CTC mode 4, toggle OC1A,  OCR1A = freq control
   TCCR1B = B00001011;  // mode 4, 011->timebase (beep on)
   OCR1A = 200;         // set freq 250->498Hz, 125->992Hz, 200->loud

// ---------------------- TIMER_2 8 bit, PWM to motors, free running ---------------
// phase correct allows 0% and 100% duty cycle, 8 MHz effective clock in
// OCR2A,B sets speed of motor
// TCCR2B PWM freq
// ------ --------
//  001   31 kHz
//  010   4 kHz         // DRV8833 gets hot
//  011   500 Hz
//  100   122 Hz
//  101   30 Hz
// TCCR2A COM2A1  COM2A0  COM2B1  COM2B0  COM2C1  COM2C0  WGM21   WGM20
// TCCR2B FOC2A   FOC2B   ------  -----   WGM22   CS22    CS21    CS20
// TIMSK2 ----    ----    ------  -----   -----   OCIE2B  OCIE2A  TOIE
   TCCR2A = B10100001;  // Mode 1, phase correct, 0->0 PWM, 255->100% PWM, timebase=32uS/tick
   TCCR2B = B00000010;  // 001->31kHz, 010->4kHz, 011->500Hz, 100->122Hz, 101->30Hz frame rate

// ---------------------------- TIMER_3 sonar, 16 bit, free running ----------------
  TCCR3A = B00000000;   // Mode 0, 16 bit, freerun
  TCCR3B = B00000011;   // /64, 250 kHz clock, 4 uS/tick, overflow in .262144 sec
  
// ---------------------------- TIMER_4 servo --------------------------------------
  TCCR4A = B10101010;   // Mode 14, fast PWM 16 bit, freerun
  TCCR4B = B00011010;   // WGM13:2=11(mode14) CS12:0=010=33mS period, start servo pulses
  ICR4   = 0xFFFF;      // TOP (freerun, 16 bits)
  OCR4A  = servoCenter; // 1500->.8mS, 2000->1mS, 3000->1.5mS, 4000->2mS
  OCR4B  = servoCenter; // servo center is determined experimentally
  OCR4C  = servoCenter; // is wheel pointed straight ahead?

// ---------------------------- TIMER_5 as periodic interrupt timer ----------------
  TCCR5A = B00000000;   // Mode 12 CTC, ICR5A = TOP
  TCCR5B = B00001010;   // 2 MHz, 
  TIMSK5 = B00000010;   // interrupt on output compare match A
  OCR5A  = 20000;       // 20,000 x .5uS = 10 mS interrupt rate
}

void initializeInterrupts() {
//----------------------------- Pin Change int -------------------------------------
// PCMSK0 PCINT7  PCINT6  PCINT5  PCINT4  PCINT3  PCINT2  PCINT1  PCINT0
// PCMSK1 PCINT15 PCINT14 PCINT13 PCINT12 PCINT11 PCINT10 PCINT9  PCINT8
// PCMSK2 PCINT23 PCINT22 PCINT21 PCINT20 PCINT19 PCINT18 PCINT17 PCINT16
// PCICR  -----   -----   -----   -----   ----    PCIE2   PCIE1   PCIE0
  PCMSK0 = B00000000; // 0-7, mask in which pins are active, PCINT2&3 are available
  PCMSK1 = B00000000; // 8-15, PCINTs in this bank are not used, some of which are available
  PCMSK2 = B00000111; // 16-23, bump on 16 & 17, pushbutton on 18, PCINT19-23 unused
  PCICR  = B00000100; // Pin Chg Int Cntrl Reg, enables 3 banks of 8 pins each, only 1 used

//  --------------------------- External Ints ---------------------------------------
// INT0,1 are same pins as I2C, INT2,3 are used by encoders, INT4 is sonar ECHO
// EIMSK  INT7    INT6    INT5    INT4    INT3    INT2    INT1    INT0
// EICRA  ISC31   ISC30   ISC21   ISC20   ISC11   ISC10   ISC01   ISC00 ints 0-3, 
// EICRB  ISC71   ISC70   ISC61   ISC60   ISC51   ISC50   ISC41   ISC40 ints 4-7
  EICRA = B01010000;  // 00=low, 01=change, 10=fall, 11= rising edge, INT2,3 encoders
  EICRB = B00000101;  // PCINT4 ping change, PCINT5 also set for change, but not enabled
  EIMSK = B00011100;  // enable INT2, INT3, INT4 (encoders & ping)
  
// ------------------------ WDT periodic timer, LASER  ----------------------------------
// nothing better to do with watchdog timer, so use as periodic interrupt generator
// MCUSR  ----     ----   ----    JTRF    WDRF    BORF    EXTRF   PORF
// WDTCSR WDIF    WDIE    WDP3    WDCE    WDE     WDP2    WDP1    WDP0
  cli();              // disable ints, special sequence required to modify wdt setup 
  MCUSR = 0;          // reset all reset source flags including WDT
  WDTCSR = B00011000; // WDE and WDCE must be set to change prescaler and WDIE, must immed write to WDTCSR
  WDTCSR = B01000010; // WDE=0, WDIE=1, WDCE=0, last 4 bits; 000-16mS, 001-32mS, 010-64mS, 125ms,.25,.5,1,2,4,8
  sei();              // enable ints
}

void initializeVariables() {
  Ks = 4;                               // Kp steer, use only even numbers, steering P term gain
  cmPerTick = wheelDia*3.14/(2*holes);  // two ticks/hole, wheel dia in cm
  float A = 1.0;                        // tan = A/B, A = one tick
  float B = trackWidth;                 // cm, next is angle turned if one wheel is stopped
  degPerTick = (A/B)*180/3.14;          // for small angles --> tan = angle in radians, radians to degrees
  deltaT = 1.0/rate;                    // used to convert ticks/deltaT to cm/sec, decimal point is needed
  speedCmd = 0;                         // set direction
  pingDist = 99;                        // so no obstacle detect before first ping
  mtrAside = 2*mtrAonLeft-1;            // +1 if motorA on left side, -1 if motorA on right side
  EEPROM.get(2, quietState);            // get value from eeprom
  if(quietState>2) quietState = 1;      // if out of range, set to soft
  EEPROM.get(0, servoCenter);           // get value from eeprom
  if((servoCenter<servoMin)||(servoCenter>servoMax)) servoCenter = 3000;// if never init, set default
  EEPROM.get(3, Kp);                   // get Kp
  if(Kp>100) Kp = 10;                   // if out of range, set to 10
  analogReference(INTERNAL1V1);         // uses 1.1V Vref
  batVolts = analogRead(vBatPin)*bitsToVolts;// initialize filter
  for(int i=0;i<99;i++) checkBat();     // stabilize battery filter, sets low bat flag
  if(batVolts>batLow) {
    batPwr = true;                      // power coming from battery
    Serial.println("Battery connected");
  }
  else {
    batPwr = false;                     // power from IDE
    Serial.println("IDE power");
  }
  checkBat();                           // flag might have gotten set before debug criteria
}

void waitForPushbuttonRelease() {             // wait for operator to release pushbutton
  boolean pushbuttonDown = digitalRead(pbPin);// read pushbutton pin, LOW(false)=released, HIGH=held down
  if(pushbuttonDown) {
    Serial.print("Please release pushbutton  ");// pushbutton is being held down
    while(pushbuttonDown) {                     // execute loop if pin is high
      delay(100);                               // debounce
      pushbuttonDown = digitalRead(pbPin);      // read pushbutton again
      Serial.print("*");                        // print clue to monitor
    }                                 // debounce if entered on a bounce
    Serial.println(" Thank you");
  }
  delay(100);
  pushbuttonFlag = false;                     // reset flag caused by interrupt on pushbutton release
}

//------------------------------- Setup ---------------------------------
void setup() {                      // bootloader 1 sec delay
  initializeIO();                   // pinmodes and turn on power to this board, release pushbutton
  initializeSerial();               // init serial comms to IDE
  initializeMtrCtrl();             // set up motor controller - different motor controllers supported
  initializeVariables();            // init variables
  initializeBeep();                 // decide how loud to beep
  initializeTimers();               // init timers, this will start beep
  if(batPwr) piPwrOn();             // power to Pi if battery connected
  else Serial.println("Pi power off");
  initializeInterrupts();           // set up isrs
  showMenu();                       // show op command menu
  delay(1000);                      // time for menu to appear on op console, beep continues
  beepOff();                        // for one second -> time to release pushbutton
  delay(1000);                      // beep off, delay so that stop double beep heard
  waitForPushbuttonRelease();       // wait for pb to be released
}

// ---------------------------- Task Scheduler --------------------------------------
// the 10 mS slot uses ~10% of processor, remaining tasks use ~10%
// increasing Periodic Interrupt Routine to 1 mS -> 45% processor time used, much faster with STM32
void loop() {                       // ~170k x/sec in forward state with 10mS PIR
  if(xps100) {                      // 100 x/sec, 10 mS
    xps100 = false;                 // reset flag
    if(Serial.available())  opCom();// respond to operator on UART1 (Arduino IDE)
    if(Serial2.available()) piCom();// Pi command comm link
    stateManager();                 // check events and change state according to events
  }                                 // PIR+these tasks take 100 uS in fwd state->10% of processing time
  if(xps50) {                       // 50x/sec, 20 mS
    xps50 = false;                  // reset flag
  }
  if(xps20) {                       // 20x/sec, 50 mS
    xps20 = false;                  // routines in this task slot take 500 uS (fwd 9 cm/s)
    pingTrigger();                  // sonar TRIG, isr will finish, 50ms->25 foot range
    calcSpeed();                    // NOTE: xps20->"int const rate=20", if changed, change rate
    steer();                        // steer active only in fwd state
    speedCtrl();                    // control speed of left/right motors
  }
  if(xps10) {                       // 10x/sec, 100 mS
    xps10 = false;                  // routines take 15 uS, 80 uS if Curve active
    if(pushbuttonFlag) piPwrOff();     // turn off, detected by PCINT18
    if(pathRadius)     followPath();// issue new bearing command if curve radius nonzero
    if(beepFlag)       beepBkgnd(); // beep control
  }    
  if(xps05) {                       // 5x/sec, 200 mS
    xps05=false;                    // reset flag
  }
  if(xps02) {                       // 2x/sec, 500 mS
    xps02 = false;                  // reset flag
  }
  if(xps01) {                       // 1x/sec, 1 second
    xps01 = false;                  // tasks take 20 uS, total task execution time (all tasks)<1mS
    seconds++;                      // increment seconds
    if(!(seconds%10)) checkBat();   // 200 uS every 10 seconds
    if(report==1) report1();        // if report=0 show nothing
    if(report==2) report2();        // select diagnostic report 2
    if(report==3) report3();
    if(report==4) report4();
    if(report==5) report5();
    if(report==6) report6();
    if(report==7) report7();
    loops = 0;                      // reset loop counter
  }
  loops++;                          // if no events this is the only statement executed
}
//    digitalWrite(scopePin, HIGH);
//    digitalWrite(scopePin, LOW);
//    digitalWrite(scopePin, !digitalRead(scopePin));

// ----------------------------- check conditions ----------------------------
// checkFwdConditions is critical because this is executed every main loop, 150k/s
void checkFwdConditions() {         // critical time spent here, 7 uS
  zoneFwd  = speedCmd/2 + speedCmd/3 + 5;// ultrasonic avoidance range f(speed), cm
  if(newPingFlag) {                 // takes several pings in a row to stop
    newPingFlag = false;            // reset flag (set by isr)
    if(pingDist < zoneFwd) {        // bumper is detected by interrupt
      obstacleCount++;              // increase consecutive pings below threshold
      if(obstacleCount>4) {         // 3 pings have signaled a real obstacle
        obstacleFlag = true;        // ping every 50 ms, 20x/s
      }                             // SR-04 has many varients, each has anomolies
    }                               // some will output a very long distance after
    else obstacleCount = 0;         // distance is ok
  }                                 // this seems to work pretty well
  if(distanceAct>distanceCmd)       // check distance
    distanceFlag = true;            // set flag
  }

void checkRevConditions() {         // reverse state
  if((distanceAct)>(distanceCmd))   // distance achieved?
    distanceFlag = true;            // if attained commanded distance->set flag
}

void checkSpinConditions() {        // spin state
  if(abs(motorDistA+motorDistB)>abs(trackCmd))// rotation achieved?
    angleFlag = true;               // works in both directions
}

void checkStopConditions() {        // bumper is detected by interrupt
  checkFwdConditions();             // show fwd conditions for diagnostics
  checkBat();                       // diagnostics->set bitsToVolts constant
}

void checkBat() {                   // 140 uS execution time
  batVolts = ((analogRead(vBatPin) * bitsToVolts) + 4*batVolts)/5;
  if((batVolts<batLow) && batPwr) batLowFlag = true;// if batt connected set battery low flag
  else batLowFlag = false;          // debugging from IDE - reset flag
}

// ---------------------- switch states depending on conditions -----------------
void stateManager() {
  switch(state)    {                // change based on state variable
    case fwdState:                  // if forward, execute following
      checkFwdConditions();         // collision, arrival or scan ahead for obstacle
      if(obstacleFlag){             // sonar? obstacle detected ahead?
        enterStopState();           // stop
      }                             // SR04 can return bogus results on occasion
      if(collisionFlag){            // bumper? detected by PCINT16 or 17
        enterStopState();           // stop
      }
      if(distanceFlag) {            // arrived?
        enterStopState();           // stop
      }
      if(batLowFlag) {              // battery low?
        enterStopState();           // this is the only place low bat action is taken
      }
      break;

    case revState:                  // exit when distance met
      checkRevConditions();         // reverse
      if(distanceFlag)  {           // arrived at target distance
        enterStopState();           // stop
      }
      break; 

    case stopState:                 // stop state
      checkStopConditions();        // monitor conditions to show on report
      break;                        // wait for command to exit stop state

    case spinState:                 // exit when rotation complete
      checkSpinConditions();        // check track angle
      if(angleFlag) {               // rotation complete?
        enterStopState();           // stop
      }
      break;

    case porState:                    // Power On Reset state from startup state=0
      enterStopState();               // stop
    break;
  }
}   

void reportCODABOPPtoPi() {
  Serial2.print("STOP ");           // send flags to Pi
  Serial2.print(collisionFlag);     // C
  Serial2.print(obstacleFlag);      // O
  Serial2.print(distanceFlag);      // D
  Serial2.print(angleFlag);         // A
  Serial2.print(batLowFlag);        // B
  Serial2.print(opCmdFlag);         // O
  Serial2.print(piCmdFlag);         // P
  Serial2.print(piComFailFlag);     // P
  Serial2.print(CR);                // CR only, no LF to Pi
}

// ---------------------------------- state entry points -----------------------
void enterStopState(){              // reset
  Serial.print("STOP ");            // message to IDE
  showFlags();                      // CODABOPP
  Serial.println();                 // new line
  if(batPwr) reportCODABOPPtoPi();  // send to pi
  if(batLowFlag) beep(9);           // lots of noise if bat low
  else           beep(2);           // beep on stop
  speedCmd = 0;                     // set speed=0
  setMtrDir(STOP,STOP);             // stop both motors
  state = stopState;                // set state variable
}

void enterFwdState(){
  stateChange();                    // reset flags dist=0, spd=0
  setMtrDir(FWD,FWD);               // set motor control signals
  state = fwdState;                 // set state variable
}

void enterRevState(){               // go backwards
  stateChange();                    // reset all flags
  setMtrDir(REV,REV);               // set motor control signals
  state = revState;                 // set state variable
}

void enterSpinState(int brg){  
  stateChange();                    // reset all flags
  speedCmd = 10;                    // set default speed
  trackCmd = brg/degPerTick;        // convert degrees to number of encoder ticks
  if(trackCmd>0) {
    setMtrDir(FWD,REV);             // CW
    servo(-1000);                   // servo() expects track error, this is just a big number
  }
  else {
    setMtrDir(REV,FWD);             // CCW
    servo(1000);                    // left
  }
  state = spinState;                // state
}

void stateChange(){
  collisionFlag   = false;          // reset all flags
  obstacleFlag    = false;
  angleFlag       = false;
  distanceFlag    = false;
  opCmdFlag       = false;
  piCmdFlag       = false;
  batLowFlag      = false;
  piComFailFlag   = false;
  motorDistB      = 0;              // reset all conditions
  motorDistA      = 0;
  lastMotorDistB  = 0;              // so that first speed calc is correct
  lastMotorDistA  = 0;
  distanceAct     = 0;
  speedCmd        = 0;              // speed zero
  bearingCmd      = 0;
  trackCmd        = 0;
  trackErr        = 0;
  pathRadius      = 0;
  obstacleCount   = 0;
  distanceCmd     = 1000000;        // 100,000 cm = forever
}

// ------------------------------ Calc speed ---------------------------------
// speed in cm/second, determination is function of time interval set in tasker
// 1 cm/sec = .022 mph
void calcSpeed() {                    // convert speed to cm/sec (deltaT factor)
  speedActA      = (motorDistA-lastMotorDistA)/deltaT;
  speedActB      = (motorDistB-lastMotorDistB)/deltaT;
  lastMotorDistA = motorDistA;        // save encoder for next time
  lastMotorDistB = motorDistB;        // save encoder for next time
  speedAct       = (speedActA + speedActB)/2;
}

// -------------------------------- Steer -------------------------------------
// modify left/right motor speeds to reduce track error
// called by drive before individual speed controllers
void steer(){                                   //  execution time
  speedCmdA = speedCmd;                         // set speed for each
  speedCmdB = speedCmd;                         // motor individually
  if(state==fwdState) {
      trackAct  = motorDistA - motorDistB;      // +error if left is ahead
      trackErr  = trackAct - trackCmd;          // error in cm
      speedCmdA = speedCmd - Ks*trackErr/10.;   // +error reduces speed to left motor
      speedCmdB = speedCmd + Ks*trackErr/10.;   // +err adds speed to right motor
      speedCmdA = constrain(speedCmdA, 0, speedCmd+speedCmdA/2);// constrain speed to 0-150%
      speedCmdB = constrain(speedCmdB, 0, speedCmd+speedCmdB/2);
      angle     = trackErrToSteerAngle(trackErr);
      servo(angle);
  }
  if((state==revState)||(state==stopState)) {
      servo(0);                                 // center servo
  }
}

// ------------------------------- Servo Steering --------------------------------
// TS-53 standard servo, 800 min, 4500 max, ~200 degrees rotation
// 3700 ticks / 200 deg = 18.5 tick/deg
void servo(int ang) {                 // rotate servo to specified angle
  int pulseWidth;                     // resulting ticks pulse to send to servo
  pulseWidth = servoCenter - ang*ticsPerDeg*(2*servoDir-1); // pulse duration  
  pulseWidth = constrain(pulseWidth,servoMin,servoMax);// limt of servo
  OCR4A = pulseWidth;                 // set servo
  OCR4B = pulseWidth;                 // set servo
  OCR4C = pulseWidth;                 // set servo
}

// ---------------------------- Follow curved path ------------------------------
// modify left/right motor distance to cause robot to follow curved path
// called 10x/sec by tasker, active if path radius is nonzero
void followPath() {
  bearingCmd = mtrAside * speedCmd*6/pathRadius;
  bearingCmd = constrain(bearingCmd, -180, 180);
  trackCmd += bearingCmd/degPerTick;  // returns number of cm difference in wheel ticks
}

// ---------------------- Convert track error to steering angle --------------------
int trackErrToSteerAngle(float err) {
  angle = atan2(err,trackWidth)*57;   // angle to steer, tan(err/TW)x57 (rad->deg)
  angle = constrain(angle, -95, 95);  // limit of servo
  return angle;                       // return steering angle
}

// ---------------------- Convert degress to track diff in cm -----------------------
//float degreesToTicks(int deg) {     // cm of wheel travel to accomplish number
//  int cm = deg/degPerCm;            // of degrees of vehicle rotation
//  int ticks = cm/cmPerTick;
//  return ticks;                     // determined by track width
//}

// ----------------------------- Trigger ping -----------------------------
void pingTrigger() {                  // 10 uS pulse width required
  digitalWrite(trigPin, HIGH);        // waste some time
  digitalWrite(trigPin, HIGH);        // waste more time
  digitalWrite(trigPin, LOW);         // this pulse width is 14 uS
}

// ----------------------------- Pi control -------------------------------
void piPwrOn(){                       // turn on Pi, takes 25 sec to boot
  digitalWrite(enPiPwrPin, HIGH);     // enable is active high with external pulldown 
  delay(50);                          // time for 5V to stabilize
  Serial2.begin(9600);                // 9600 Baud to Pi
  Serial2.setTimeout(50);             // timeout xx ms (comms from Pi)
  digitalWrite(piOnPin, HIGH);        // Pi shutdown signal pin high, set high first, then OUTPUT
  pinMode(piOnPin, OUTPUT);           // input until now, high->signal to Pi not to shutdown
  Serial.println("Pi power on");
}

void piPwrOff() {                     // shutdown timer
  int seconds = 12;                   // number of seconds before Pi power off
  int countdown = seconds * 10;       // counter increments in tenths seconds
  Serial.println("Pi shutting down"); // start shutdown, show on IDE
  digitalWrite(piOnPin, LOW);         // signal to Pi to start shutdown, set to low output
  Serial2.end();                      // turn off communications to Pi (need to tristate TX pin)
  for(int i=0; i<(seconds*10); i++) { // shutdown loop, 10x/s
    if((countdown%10)==0) {           // check countdown, if no remainder show count
      Serial.print(countdown/10);     // show count
      Serial.print(" ");              // space
    }                                 //
    countdown--;                      // decrement countdown
    delay(50);                        // blink status
    beepOff();                        // beep off
    delay(50);                        // .1 sec through this loop
    beepOn();                         // beep on
  }                                   // countdown loop
  Serial.println();                   // note - must tristate all outputs to Pi before pi pwr off
  Serial.end();                       // regain control of TX pin
  pinMode(tx2pin,  INPUT);            // tristate output, external resistor will pull it low
  pinMode(rx2pin,  INPUT);            // probably redundant
  pinMode(piOnPin, INPUT);            // tristate output
  digitalWrite(enPiPwrPin, LOW);      // turn off Pi 5 Volt supply 
  Serial.println("Pi off");           // power off
  delay(1000);                        // 5V supply to Pi drained
  Serial.println("Arduino off");      // shut off power to this board
  delay(20);                          // wait for msg to print
  beepOff();                          // silence!
  pushbuttonFlag = false;             // reset flag 
  WDToff();                           // WDT off
  digitalWrite(en5vPin, LOW);         // power off to this board
  while(true) {                       // hang here while power dies (if IDE power, power never dies)
    digitalWrite(lazPin, HIGH);       // laser off
    delay(1000);                      // enuf time for power to die if on battery
    if(pushbuttonFlag) reset();       // reset if pushbutton pressed
  }
}

void reset() {        // resets in 16 mS
  cli();              // disable ints, special sequence required to modify wdt setup 
  MCUSR = 0;          // reset all reset source flags including WDT
  WDTCSR = B00011000; // WDE and WDCE must be set to change prescaler and WDIE, must immed write to WDTCSR
  WDTCSR = B00001000; // WDIE=0, WDE=0, 000-16mS
  sei();
}

void WDToff() {       // disables WDT
  cli();              // disable ints, special sequence required to modify wdt setup 
  MCUSR = 0;          // reset all reset source flags including WDT
  WDTCSR = B00011000; // WDE and WDCE must be set to change prescaler and WDIE, must immed write to WDTCSR
  WDTCSR = B00000000; // WDE=0, WDIE=0, 000-16mS
  sei();
}

// ----------------------------- Beep -----------------------------------
// this sets up variables so that actual beeps are handled in task scheduler
void beep(byte num_beeps) {           // number of beeps
  beepNumber = num_beeps;             // beep + silence
  beepFlag = true;                    // tasker will invoke beepBkgnd()
  beepOnOffFlag = true;               // first beep cycle is beeper on
}

void beepBkgnd() {                    // called in tasker
  if(beepOnOffFlag) {                 // beep does not hang tasks
    beepOn();                         // beeper on
    beepNumber--;                     // decrement beep counter
  }
  else {
    beepOff();                        // beep off during interbeep time
    if(beepNumber==0) beepFlag = false;// last beep
  }
  beepOnOffFlag = !beepOnOffFlag;     // beep or silence between multiple beeps
}

void beepOff() {                      // stop timer
  TCCR1B = 0;                         // set timer clock divisor = no clock
}

void beepOn() {                       // start timer
  if(quietState==0) return;               // do not beep
   TCCR1A = B01000000;                // CTC mode 4, toggle OC1A,  OCR1A = freq control
   TCCR1B = B00001011;                // mode 4, 011->timebase (beep on)
}


// -------------------------------  operator interface -------------------------------------
// if all command args are not sent, tasker will hang here for Serial.setTimeout time (500 mS)
void opCom() {
  int inNum;  
  char inChar;
  inChar = Serial.read();                       // get char from incoming serial stream
  if((inChar==13)||(inChar==10)||(inChar==32)) return;// ignore <CR>, <LF>, <SP>
  if(inChar > 96) inChar = inChar & B01011111;  // transform lower case to upper case
  switch(inChar)    {
     int spd;
     int hdg;
     long dist;

    case 'F':                             // enter forward state
      Serial.print("FORWARD ");           // "f" -> forward state
      enterFwdState();                    // change state
      break;         
    case 'B':                             // enter reverse state
      Serial.print("BACKUP ");            // "b" -> reverse state
      enterRevState();                    // change state
      break;
      
    case 'S':                             // enter stop state, brakes on
      opCmdFlag = true;                   // "s" -> stop state
      enterStopState();                   // will print "stop" and reason
      break;
      
    case 'H':                             // Halt, set V=0, do not change state
      speedCmd = 0;                       // set speed = 0
      Serial.print("HALT ");              // 
      Serial.print(speedCmd);             // 
      Serial.println(" cm/sec");          // 
      break;
    
    case 'V':                             // set velocity
      inNum = Serial.parseInt();          // "v22"
      inNum = abs(inNum);                 // NOTE - set velocity does not change state
      Serial.print("VELOCITY ");          // 
      Serial.print(inNum);                // 
      Serial.println(" cm/sec");          // 
      speedCmd = (inNum);                 // 
      break;
    
    case 'C':                             // curvature - set path radius, cm
      pathRadius = Serial.parseInt();     // "c100" will cause robot to circle 100 cm radius
      pathRadius = constrain(pathRadius, -200, 200);
      Serial.print("CURVE RADIUS ");
      Serial.print(pathRadius);
      Serial.println(" cm");              // cm
      break;
    
    case 'D':                             // set distance to travel, cm
      inNum = Serial.parseInt();          // "d100" robot will stop 100 cm from here
      distanceCmd = (inNum);
      Serial.print("DISTANCE ");          // cm
      Serial.print(inNum);
      Serial.println(" cm");
      break;
    
    case 'L':                             // Turn -> modify heading
      inNum = Serial.parseInt();          // "L90" will turn 90 deg ccw
      inNum = -abs(inNum);
      inNum = constrain(inNum, -360, 0);
      Serial.print("LEFT ");              // if turn when stopped -> spin
      Serial.print(inNum);
      Serial.println(" deg");             // deg    
      bearingCmd = inNum * mtrAside;      // degrees, sign depends on motor config
      trackCmd += bearingCmd/degPerTick;  // returns number of cm difference in wheel ticks
      if(state==stopState) {              // if stopped, then spin
        motorDistA = 0;
        motorDistB = 0;
        
        enterSpinState(bearingCmd);       // entry into spin state
      }
    break;
    
    case 'R':                             // Turn -> modify heading
      inNum = Serial.parseInt();          // "r90" will turn 90 deg cw
      inNum = abs(inNum);                 // get turn command in degrees
      inNum = constrain(inNum, 0, 360);   // limit to 360
      Serial.print("RIGHT ");             // if turn when stopped -> spin
      Serial.print(inNum);                // show
      Serial.println(" deg");             // degrees  
      bearingCmd = inNum * mtrAside;      // get number of degrees
      trackCmd += bearingCmd/degPerTick;  // returns reqd difference in wheel ticks   
      if(state==stopState) {              // if stop state -> spin
        motorDistA = 0;                   // else track cmd will handle in fwd state
        motorDistB = 0;                   // command is ignored in reverse state
        enterSpinState(bearingCmd);       // entry into spin state
      }
    break;
    
    case 'K':                             // kill power to everything
      Serial.print("System power off ");  // "K" will turn off power
      piPwrOff();                            // start power off sequence
      break;
    
    case 'P':                             // set speed control proportional term
      Serial.print("Kp ");                // "p5" will set Kp=5
      Kp = Serial.parseInt();
      Serial.println(Kp);
      EEPROM.put(3, Kp);                  // store it      
      Serial.println();
      break;
    
    case 'A':                             // Adjust servo center
      Serial.print("Adjust ");            // "A-100" will subract 100 ticks from current center
      inNum = Serial.parseInt();          // if 0 or no arg, servo center is unchanged
      Serial.print(inNum);
      servoCenter += inNum;
      Serial.print(" Center ");
      Serial.print(servoCenter);
      EEPROM.put(0,servoCenter);
      Serial.println();
      break;

    case 'Q':                             // quiet mode
      quietState++;                       // increment quiet state
      if(quietState>2) quietState = 0;    // = 0,1,2
      initializeBeep();
      EEPROM.put(2,quietState);
      beep(3);                            // beep 3 times, releases control to tasker
      break;
    
    case 'X':                             // select diagnostic report, enter values 1-7 to
      Serial.println();
      Serial.print("Report ");             // "p5" will set Kp=5
      report = Serial.parseInt();          // select desired report to Arduino IDE monitor e.g."d3"
      if(report==0) Serial.print("0, Reports off");
      if(report==1) Serial.print("1, Motors");
      if(report==2) Serial.print("2, Steering");
      if(report==3) Serial.print("3, General");
      if(report==4) Serial.print("4, Ping");
      if(report==5) Serial.print("5, Spin");
      if(report==6) Serial.print("6, Power control");
      if(report==7) Serial.print("7, Mechanical");
      Serial.println();
      lineCounter = 0;
      break;
       
    case 'M':                             // Menu
      showMenu();
      report = 0;
      break;
...

This file has been truncated, please download it to see its full contents.