Robot controlled with muscles via radio-enabled EMG sensors

#include <SPI.h>
#include "uMyo_RF24.h"

#include <FastLED.h>

int rf_cen = 10; //nRF24 chip enable pin
int rf_cs = 9; //nRF24 CS pin

byte dirL = 0;
byte dirR = 0;
byte speed_high = 250;

byte motL_a = 2; //PD2
byte motL_b = 3; //PD3

byte motR_a = A0; //PC0
byte motR_b = A1; //PC1

byte motL_on_mask = 0b1000;
byte motL_off_mask = 0b11110011;
byte motR_on_mask = 0b10;
byte motR_off_mask = 0b11111100;

byte motL_fwd_mask = 0b1000;
byte motR_fwd_mask = 0b10;
byte motL_bwd_mask = 0b0100;
byte motR_bwd_mask = 0b01;

byte en_L = 5;
byte en_R = 6;

byte sys_on = 8;

byte led_pin = 7;

volatile uint32_t ovf_cnt = 0;

CRGB leds[6];

void out_led_state()
{
  FastLED.show();
}


void startTimer2()
{
  TCCR2A = 0b00000000;
  TCCR2B = 0b00000100; //64 prescaler, 256*64/16MHz = 1.024 ms period with 256 top

  TIMSK2 = 0b111;// _BV(OCIE1A | OCIE1B | TOIE1); //turn on timer interrupts
}

ISR(TIMER2_COMPA_vect) 
{
  PORTD &= motL_off_mask;
  ovf_cnt++;
  return;
}
ISR(TIMER2_COMPB_vect) 
{
  PORTC &= motR_off_mask;
//  ovf_cnt++;
  return;
}
ISR(TIMER2_OVF_vect) 
{
  PORTD |= motL_on_mask;
  PORTC |= motR_on_mask;
//  ovf_cnt++;
//  startTimer1();
  return;
}

void setup() {
  Serial.begin(115200);
//  Serial.begin(921600);
  pinMode(motL_a, OUTPUT);
  pinMode(motL_b, OUTPUT);

  pinMode(motR_a, OUTPUT);
  pinMode(motR_b, OUTPUT);

  pinMode(en_L, OUTPUT);
  pinMode(en_R, OUTPUT);

  pinMode(sys_on, OUTPUT);

  digitalWrite(motL_a, 0);
  digitalWrite(motL_b, 0);
  digitalWrite(motR_a, 0);
  digitalWrite(motR_b, 0);
  digitalWrite(en_L, 0);
  digitalWrite(en_R, 0);
  digitalWrite(sys_on, 1);

  FastLED.addLeds<NEOPIXEL, 7>(leds, 6);  // GRB ordering is assumed

  for(int x = 0; x < 6; x++)
    leds[x] = CRGB(0, 25, 0);
  FastLED.show();
  delay(300);
  for(int x = 0; x < 6; x++)
    leds[x] = CRGB(0, 2, 0);
  FastLED.show();
  uMyo.begin(rf_cs, rf_cen);
  startTimer2();
}

void ampl_to_color(int ampl, uint8_t *r, uint8_t *g, uint8_t *b)
{
  if(ampl == 0) { *r = 1; *g = 1; *b = 1; return; }
  int T1 = 60;
  int T2 = 120;
  int T3 = 180;
  int T4 = 240;
  int max_v = 20;
  if(ampl < T1)
  {
    *b = (ampl * max_v / T1);
    *g = 0;
    *r = 0;
    return;
  }
  if(ampl < T2)
  {
    int ca = ampl - T1;
    *b = (max_v - (ca * max_v / (T2-T1)));
    *g = (ca * max_v / (T2-T1));
    *r = 0;
    return;
  }
  if(ampl < T3)
  {
    int ca = ampl - T2;
    *b = 0;
    *g = (max_v - (ca * max_v / (T3-T2)));
    *r = (ca * max_v / (T3-T2));
    return;
  }
  if(ampl < T4)
  {
    int ca = ampl - T3;
    *g = 0;
    *r = (max_v);
    *b = (ca * max_v / (T4-T3));
    return;
  }
  *r = (max_v);
  *b = (max_v);
  *g = 0;
}

uint32_t mot_set_ms = 0;

void set_motors(int spdL, int spdR)
{
  uint32_t ms = millis();
  if(ms - mot_set_ms < 30) return;
  char buf[64];
  sprintf(buf, "%d  %d\n", spdL, spdR);
  Serial.print(buf);
  
  if(spdL < 0)
  {
    motL_on_mask = motL_bwd_mask;
    spdL = -spdL;
  }
  else 
    motL_on_mask = motL_fwd_mask;
  if(spdR < 0)
  {
    motR_on_mask = motR_bwd_mask;
    spdR = -spdR;
  }
  else 
    motR_on_mask = motR_fwd_mask;
  mot_set_ms = ms;
  uint8_t r, g, b;
  ampl_to_color(spdL, &r, &g, &b);
  leds[0] = CRGB(r, g, b);
  leds[1] = leds[0];
  leds[2] = leds[0];

  ampl_to_color(spdR, &r, &g, &b);
  leds[5] = CRGB(r, g, b);
  leds[4] = leds[5];
  leds[3] = leds[5];
  out_led_state();
//  return;
  if(spdL == 0) OCR2A = 1;
  else if(spdL > 255) OCR2A = 255;
  else OCR2A = spdL;
  if(spdR == 0) OCR2B = 1;
  else if(spdR > 255) OCR2B = 255;
  else OCR2B = spdR;
}

float lvl_l = 0;
float lvl_r = 0;

byte idxL = 255;
byte idxR = 255;

void loop() {
  uint32_t ms = millis();
  uMyo.run();
  int N = uMyo.getDeviceCount();
  if(N < 2)
  {
    set_motors(0, 0);
    return;
  }
  if(idxL > 250) //initialization, determine left/right hands indexes (idxL, idxR) by their orientation
  {
    float pitch0 = uMyo.getPitch(0);
    float pitch1 = uMyo.getPitch(1);
    if(pitch0 > 0 && pitch1 < 0)
    {
      idxL = 0;
      idxR = 1;
    }
    else
    {
      idxL = 1;
      idxR = 0;
    }
  }
  lvl_l *= 0.95;
  lvl_l += 0.05*uMyo.getMuscleLevel(idxL);
  lvl_r *= 0.95;
  lvl_r += 0.05*uMyo.getMuscleLevel(idxR);
  
  float pitchL = uMyo.getPitch(idxL);
  float pitchR = uMyo.getPitch(idxR);
  int spdL = uMyo.getMuscleLevel(idxL);//lvl_l;// - 300;
  int spdR = uMyo.getMuscleLevel(idxR);//lvl_r;// - 300;

  if(spdL > speed_high) spdL = speed_high;
  if(spdR > speed_high) spdR = speed_high;
  if(pitchL < 0.1) spdL = -spdL; //reverse direction if arms are looking down
  if(pitchR > -0.1) spdR = -spdR;
  set_motors(spdL, spdR);
}