facilities:hte:hte_prototypes:brickbattlebots:code

// Version 0.14
// Now with servo and new neopixel library
// Two 'weapon' surport.
// More code tidyups
// Robot jumper mode select

// USB Host shield
#include <PS3BT.h>
#include <usbhub.h>
#include <SPI.h>

// Built in servo library
#include <Servo.h>

//Uncomment one of these
//#include <Adafruit_NeoPixel.h>
#include <WS2812.h>   // https://github.com/cpldcpu/light_ws2812

// Uncomment depending on style of bot
//#define SKIDBOT
//#define STEERBOT
// 0=Test only.   1=Skidsteer bot,   2=Steering style bot
char bottype=1;

//Setup serial port and debug timer
#define SERIAL_PORT_SPEED 115200
#define DEBUGVALUE 500  // Every DEBUGVALUE trigger debug code
unsigned long debugtimer;
unsigned long loopmills;

// Reduses controller jitter
#define DEADZONE 8

// H-Bridge motor controller for DC motors
// motor one
#define pwmA 5    // Must be a PWM pin
#define aIN1 2    // H-Bridge
#define aIN2 3    // Control pins
// motor two
#define pwmB 6   // PWM
#define bIN1 4
#define bIN2 7

// Servos
#define SERVOONEPIN 8   // Where are they connected
#define SERVOTWOPIN A4
Servo servo_one;        // create servo object to control a servo
Servo servo_two;  

// Battery monitoring analog input pin
#define VINPIN A0

int dirval,speedval,weppval,auxval;                // Inputs
int left_motor,right_motor,webb_motor,aux_motor;   // Outputs

int motcurrmax;               // Peek control value
int battvolt;                 // Battery voltage

// Spinner style weapon spinup delays. Prevents motor stripping and looks more dramatic
#define WEPPTIMERVALUE 100  // How often (in ms) to update speed
#define WEPPRATEDELTA 10   // Weapon spin up speed, units per WEPPTIMERVALUE
#define WEPPMAX 200   // Weapon max speed
unsigned long wepptimer;
int currweppspeed;

// Settings and timeouts for 'boost'.
// Motors are 6v and this allows tempory driving to full battery voltage
#define HALFPOWERTIMEOUT 10000    // In milis.
#define HALFPOWERCOOLDOWN -20000
#define HALFPOWER 170   //  (actually halfish)     Limit motor to 2/3 power:  (255/3)*2
long halfpowertimer;
int motormax=HALFPOWER;

// 0=Debug: No movement, all controlls displayed.
// 1=Single stick: Right stick, X axis Steer.  Y axis throttle.
// 2=Two stick: Left stick Y axis Throttle. Right stick X axis steering.
// 3=D-Pad: For crazy people. Digital only for now, May add preassure sense.  (Not working)
// 4=Hayden: Right trigger forward, left trigger backward, left stick X steering.
int controlmode=1;

// Main RGB LEDs
#define PIXELPIN  A1 
#define NUMPIXELS  2
//Adafruit_NeoPixel neopixel = Adafruit_NeoPixel(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ400);
WS2812 LED(NUMPIXELS); // light_ws2812 

// USB Host Shield object setup 
USB Usb;
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
PS3BT PS3(&Btd); // This will just create the instanceinstance

// Runs just once
void setup() {

  // Test to see if Tx and Rx have been shorted by a jumper.
  // This sets the robot's mode:  Jumper off: Skid.  Jumper on: Steer
  pinMode(1, OUTPUT);        // Tx pin
  pinMode(0, INPUT_PULLUP);  // Rx pin
  setPixelRGB(0, 00, 30, 00);
  digitalWrite(1, LOW);  // Pull Tx LOW
  delay(10);
  if(digitalRead(0)==LOW) {  // Read Rx
    digitalWrite(1, HIGH);  // If low, take Tx HIGH
    delay(10);
    if(digitalRead(0)==HIGH) {  // See if state changed
      bottype=2;   // Change robot mode.
    } 
  }
  
  // Serial port setup with delay to connect
  Serial.begin(SERIAL_PORT_SPEED);
  delay(500);
  Serial.print("Starting... Bot Mode: ");  // Just so we know where we are
  Serial.println(bottype,DEC);
  
  // RGB led library setup
//  neopixel.begin();
  LED.setOutput(PIXELPIN);

// Lights depend on bot type to show we have started
switch (bottype){
  case 0:   // Test/debug
    setPixelRGB(0, 30, 30, 30);  // White
  break;
  case 1:  // Skidbot
    setPixelRGB(0, 30, 30, 00);  // Green
  break;
  case 2:   // Steerbot
    setPixelRGB(0, 00, 30, 30);  // Yellow
  break;
}
  setPixelRGB(1, 00, 00, 00);


  // Give the user a moment to see the LEDs
  delay(1000);

  // set all the motor control pins to outputs and make sure motors are off
  pinMode(pwmA, OUTPUT);
  pinMode(pwmB, OUTPUT);
  pinMode(aIN1, OUTPUT);
  pinMode(aIN2, OUTPUT);
  pinMode(bIN1, OUTPUT);
  pinMode(bIN2, OUTPUT);
  setmotor(0,0);
  setmotor(1,0);  

  // attach each servo to the servo object and set servos to halfway
  servo_one.attach(SERVOONEPIN); 
  servo_two.attach(SERVOTWOPIN);   
  setservo(0,0);
  setservo(1,0);

  // Start USB subsystem
  if (Usb.Init() == -1) {
    Serial.print("\r\nOSC did not start");   // Very bad. You will not robot today
    setPixelRGB(0, 255, 00, 00);  // both lights red
    setPixelRGB(1, 255, 00, 00);
    while (1); // halt - no point going further, hardware fault
  }
  Serial.print("\r\nWaiting for pairing..");  // Actually, just 'usb is working'
  
}

// Around and around and around
void loop() {
  // For debug and weapon timeouts
  loopmills=millis();

  // Uncomment only to test motors. This disables everthing else
// dcmot_test();

  // Make sure this runs fairly often. USB and bluetooth both need this
  Usb.Task();
  
  // No bluetooth connection
  if (!PS3.PS3Connected) {
    //turn off or center all motors
    setmotor(0,0);
    setmotor(1,0);
    setservo(0,0);
    setservo(1,0);
    // Turn on blue LED, others off
    setPixelRGB(0, 00, 00, 50);
    setPixelRGB(1, 00, 00, 00);
    // Wait (And around we go!)
    return;
  }

  // Boost on
  if (PS3.getButtonClick(TRIANGLE)) {
    if(halfpowertimer==0){
      motormax=255;
      PS3.setRumbleOn(RumbleLow);  // A DS3 will have fun here
      Serial.println(F("Boost! ON."));
      halfpowertimer=HALFPOWERTIMEOUT;
    }
  }

  // Manual Boost off
  if (PS3.getButtonClick(CROSS)) {
    motormax=HALFPOWER;
    if(halfpowertimer>0){  // Reward turning off boost before timeout
      halfpowertimer=-((HALFPOWERTIMEOUT-halfpowertimer)*2);     // Timer is twice what remaINs on the timer 
//      halfpowertimer=HALFPOWERCOOLDOWN;   // Disable timer 
    }
//    halfpowertimer=0;   // Uncomment to disable cooldown timer
    PS3.setRumbleOff();
    Serial.println(F("Boost! OFF."));
  }
 
  // Boost timeout
  if(halfpowertimer==1){
    halfpowertimer=HALFPOWERCOOLDOWN;   // Disable timer
    halfpowertimer=halfpowertimer*2;
    motormax=HALFPOWER;   
    PS3.setRumbleOff();
    Serial.println(F("Boost! AUTO OFF."));
  }else if(halfpowertimer>0){  // Boost timeout
    halfpowertimer--;
  }else if(halfpowertimer<0){  // Cooldown timeout
    halfpowertimer++;
  }

  // Select control mode and display on PS3's LEDs
  if (PS3.getButtonClick(SELECT)) {
    controlmode++;
    if(controlmode>4){ controlmode=0; }
    PS3.setLedOff();
    switch (controlmode){
      case 0:   
      break;
      case 1:
        PS3.setLedOn(LED1);
      break;
      case 2:
        PS3.setLedOn(LED2);
      break;
      case 3:
        PS3.setLedOn(LED3);
      break;
      case 4:
        PS3.setLedOn(LED4);
    }
  }



// Servo weapon stuff.
// And 'aux' stuff
  if (PS3.getButtonPress(UP)) {   // While button is being pressed, allow spin up to happen slowly
     weppval=WEPPMAX;
  } else if(PS3.getButtonPress (DOWN)) {  // Spin down fast
     weppval=0;
     webb_motor=0;
  } else {
    weppval=0;  // Spin down slow
   }

  // Spin weapons up towards full speed and down as needed to prevent gearbox damage
  if(wepptimer<loopmills){   // Timer just for updating this
    if(webb_motor<weppval){
      webb_motor=webb_motor+WEPPRATEDELTA;
    } else if(webb_motor>0) {
     webb_motor=webb_motor-WEPPRATEDELTA;
    }
    wepptimer=loopmills+WEPPTIMERVALUE;  //  reset timer
  }
      
// servo aux goes here!

// end

  // Input debugging, also check battery voltage
  if(debugtimer<loopmills){
    // Devider network:  GND---4.6k---A0---8.2k----Vbatt
    // Values: 9v = 671,  9.5v=712, 11.1v=833, 12.6v=941
    Serial.print(F("V:")); 
    battvolt= analogRead(VINPIN);
    battvolt = map(battvolt, 671, 941, 0,254 );
    Serial.print(battvolt);
//    battvolt = 20;    //   Override for hardware testing.   ** COMMENT OUT THIS LINE BEFORE USE **
    // Battery too low, disable motors, flash first neopixel red
    if(battvolt<10){  // Red flash
 //   while (PS3.getButtonClick(TRIANGLE)){    // <-- What is this for?  (Some old debug code?)
      setPixelRGB(0, 00, 00, 00);
      setPixelRGB(1, 00, 00, 00);
      delay(200);
      setPixelRGB(0, 60, 00, 00);
      setPixelRGB(1, 00, 00, 00);
      motormax=0;
    } else if(battvolt<50) {  // Battery just low: *Red*
      setPixelRGB(0, 60, 00, 00);

    } else if(battvolt<100) {  // Battery OKish: *Orange*
      setPixelRGB(0, 60, 30, 00);
    } else {  //  Battery is fine! So: *Green*
      setPixelRGB(0, 00, 30, 00);
    }
    Serial.print(F(" M:"));
    Serial.print(controlmode,DEC);  // Why does this not display!?!?
    switch (controlmode){
      case 0:
        Serial.print(F(" RHx:"));
        Serial.print(PS3.getAnalogHat(RightHatX));
        Serial.print(F(" RHy: "));
        Serial.print(PS3.getAnalogHat(RightHatY));
        Serial.print(F(" LHy:"));
        Serial.print(PS3.getAnalogHat(LeftHatY));
        Serial.print(F(" LHx:"));
        Serial.print(PS3.getAnalogHat(LeftHatX));
        Serial.print(F(" L2:"));
        Serial.print(PS3.getAnalogButton(L2));
        Serial.print(F(" R2:"));
        Serial.print(PS3.getAnalogButton(R2));
        Serial.print(F("\t"));      
      break;
      case 1:
        Serial.print(F(" RHx:"));
        Serial.print(PS3.getAnalogHat(RightHatX));
        Serial.print(F("\tRHy: "));
        Serial.print(PS3.getAnalogHat(RightHatY));
      break;
      case 2:
        Serial.print(F(" LHy:"));
        Serial.print(PS3.getAnalogHat(LeftHatY));
        Serial.print(F(" RHx:"));
        Serial.print(PS3.getAnalogHat(RightHatX));
      break;
      case 3:
      break;
      case 4:
        Serial.print(F(" LHx:"));
        Serial.print(PS3.getAnalogHat(LeftHatX));
        Serial.print(F(" L2:"));
        Serial.print(PS3.getAnalogButton(L2));
        Serial.print(F(" R2:"));
        Serial.print(PS3.getAnalogButton(R2));        
      break;  
  }
    Serial.print(F("\t "));
  }

// Actually read in values from controller
// And bring things into pos/neg
  switch (controlmode){ 
    case 1: // Default: 1=Right stick. Cent off, X=Steer.  Y=Speed.
      dirval=PS3.getAnalogHat(RightHatX)-128;
      speedval=PS3.getAnalogHat(RightHatY)-128;
      // Invert axis if needed
      speedval = -speedval;
     // dirval =-dirval;
    break; 
    case 2: // 2=Genric 2 stick: left stick Y axis Throttle. Right stick X axis steering.
      dirval=PS3.getAnalogHat(RightHatX)-128;
      speedval=PS3.getAnalogHat(LeftHatY)-128;
      // Invert axis if needed
      speedval = -speedval;
     // dirval =-dirval;   
    break;
    case 3:  // 3=D-Pad
      if (PS3.getButtonClick(UP)) { }
      if (PS3.getButtonClick(RIGHT)) { }
      if (PS3.getButtonClick(DOWN)) { }
      if (PS3.getButtonClick(LEFT)) { }
      dirval=0;
      speedval=0;
    break; 
    case 4: // 4=Hayden: left backward, Right forward, left stick X axis steering.
      dirval=PS3.getAnalogHat(LeftHatX)-128;
      speedval=PS3.getAnalogButton(L2);
      speedval=speedval-PS3.getAnalogButton(R2);
      speedval = map(speedval, -255, 255, 128,-128 );
     // speedval = -speedval;
     // dirval =-dirval;    
    break;
  }
 
  // Dead zone, to reduce twitching.
  if(abs(dirval)<DEADZONE){
    dirval=0;
  }
  if(abs(speedval)<DEADZONE){
    speedval=0;
  }

//  dirval = map(dirval, 128, -128, 50,-50 );

  // What do we have in the regs?
  if(debugtimer<loopmills){
    Serial.print("D:"); Serial.print(dirval); Serial.print("\t");
    Serial.print("S:"); Serial.print(speedval); Serial.print("\t");
    Serial.print("W:"); Serial.print(weppval); Serial.print("\t");
    Serial.print("A:"); Serial.print(auxval); Serial.print("\t");
  }



switch (bottype){

  case 0:   // Test/debug
  break;


  case 1:  // Skidbot
/*
  // Byron's janky Two channels to diffrential
  if(speedval>1){    // Forward
    left_motor=speedval-dirval;
    right_motor=speedval+dirval;
  }
  else if(speedval<-1){  // Backward
    left_motor=speedval+dirval;
    right_motor=speedval-dirval; 
  }
  else{   // Stopped
    left_motor=0;
    right_motor=0;
  }  
*/

  //https://www.reddit.com/r/arduino/comments/49nltm/differential_steering_mixingformula/d0tdibk/
  // Much better diffrential, bodged to work.
  // Still not great at low-but-non-zero speeds
  /*
  if (speedval == 0 && dirval !=0) {
    left_motor = dirval;
    right_motor = -dirval;
  } else {
    left_motor = speedval * ((-128 - dirval) / -128.0);
    right_motor = speedval * ((128 - dirval) / 128.0);

    if (speedval > 0 && left_motor > speedval)
      left_motor = speedval;
    if (speedval > 0 && right_motor > speedval)
      right_motor = speedval;
    if (speedval < 0 && left_motor < speedval)
      left_motor = speedval;
    if (speedval < 0 && right_motor < speedval)
      right_motor = speedval;
  }
*/


  /* Kell example:   Scale tune to be good.  Offsets should be zero
  motor_r = scale*(forward - forward_offset + turn - turn_offset);
  motor_l = scale*(-forward + forward_offset + turn - turn_offset);
  */
  right_motor = (speedval  + dirval );
  left_motor = (-speedval + dirval );
break;
case 2:   // Steerbot - servo steering.
  left_motor = speedval;
  right_motor = dirval;
break;

}
    
    // Show current motor staus on LEDs
    motcurrmax=(abs(speedval)*1.55);    // Scale 0-128 to 0-255. I think.
    if(motormax>HALFPOWER){  // Orange
      if(motcurrmax>113){ // In 'danger zone'
        setPixelRGB(1, motcurrmax+10, 248-(motcurrmax*1.25), 00);
      } else {  // 'Boost' enabled but not in zone
        setPixelRGB(1, motcurrmax+10, motcurrmax+10, 00);
      }
    } else{  // Green only
      setPixelRGB(1, 00, motcurrmax+10, 0);
    }     

  // rescale from +/- 128 to +/- motormax, to allow for boost stuff
  left_motor = map(left_motor, 128, -128, motormax,-motormax);
  right_motor = map(right_motor, 128, -128, motormax,-motormax);

  // Lets see what is going to the motors
  if(debugtimer<loopmills){
    debugtimer=loopmills+DEBUGVALUE;  // Last 'debug if': reset timer
    Serial.print("LED:"); Serial.print(motcurrmax); Serial.print("\t");
    Serial.print("L:"); Serial.print(left_motor); Serial.print("\t");
    Serial.print("R:"); Serial.print(right_motor); Serial.print("\t");
    Serial.print("W:"); Serial.print(webb_motor); Serial.print("\t");
    Serial.print("A:"); Serial.print(aux_motor); Serial.print("\t");
    Serial.print("B:"); Serial.print(halfpowertimer); Serial.print("\n");
  }


// Different motor configs

switch (bottype){

  case 0:   // Test/debug
  break;
  case 1:  // Skidbot
    // Send to motors, 
    setmotor(0,left_motor);
    setmotor(1,right_motor);   // Oooh! This is why it was this way!
    // And servo(s)
    setservo(0,webb_motor);
    setservo(1,aux_motor);
  break;
  case 2:   // Steerbot
    // Send to motors, 
    setmotor(0,left_motor);  // Actually 'forward'
    setmotor(1,webb_motor);
    // And servo(s)
    setservo(0,right_motor);   // Odd but hey, this is how it's written
    setservo(1,aux_motor);
  break;
}



}  // loop() end


// Set H bridge motor output controls and PWM speeds.
// 255 = full forward.  -255 full reverse
void setmotor(char mot,int mot_speed){
  constrain(mot_speed,-255,255);   // Prevent values from going out of range
  // Left motor
  if(mot==0){   
    if(mot_speed>0){           // Forward
      digitalWrite(aIN1, LOW);
      digitalWrite(aIN2, HIGH);
      analogWrite(pwmA, abs(mot_speed));   // PWM output for motor speed
    } else if(mot_speed<0) {   // Backward
      digitalWrite(aIN1, HIGH);
      digitalWrite(aIN2, LOW);
      analogWrite(pwmA, abs(mot_speed));  // PWM output for motor speed
    } else {  // Break/stop
      digitalWrite(aIN1, LOW);
      digitalWrite(aIN2, LOW);
      analogWrite(pwmA, 0);
    }
  }
  // Left motor
  if(mot==1){           
    if(mot_speed>0){               // Forward
      digitalWrite(bIN1, LOW);
      digitalWrite(bIN2, HIGH);
      analogWrite(pwmB, abs(mot_speed));  // PWM output for motor speed
    } else if(mot_speed<0) {       // Backward
      digitalWrite(bIN1, HIGH);
      digitalWrite(bIN2, LOW);
      analogWrite(pwmB, abs(mot_speed));  // PWM output for motor speed
    } else {  // Break/stop
      digitalWrite(bIN1, LOW);
      digitalWrite(bIN2, LOW);
      analogWrite(pwmB, 0);
    }
  }
}



// Turn -128 128   range into servo output
void setservo(char mot,int servpos){
    constrain(servpos,-128,128);   // Prevent values from going out of range
//    servpos = map(servpos, -128, 128, 5,175 );  // And convert to servo range
    servpos = map(servpos, -128, 128, 50,130 );  // And convert to servo range

  if(mot==0){   
    servo_one.write(servpos);
  }
  if(mot==1){   
    servo_two.write(servpos);
  }

  
}
    
// Motor test function,
void dcmot_test(){
  
  //Mot one
  setPixelRGB(0, 00, 30, 00);  // Forward
  setmotor(0,128);
  delay(1000);
  setPixelRGB(0, 00, 00, 00); 
  setmotor(0,0);
  delay(500);
  setPixelRGB(0, 30, 00, 00);  //Backward
  setmotor(0,-128);
  delay(1000);
  setPixelRGB(0, 00, 00, 00); 
  setmotor(0,0);
  delay(1000);
   
  // Mot two
  setPixelRGB(1, 00, 30, 00);
  setmotor(1,128);
  delay(1000);
  setPixelRGB(1, 00, 00, 00); 
  setmotor(1,0);
  delay(500);
  setPixelRGB(1, 30, 00, 00);
  setmotor(1,-128);
  delay(1000);
  setPixelRGB(1, 00, 00, 00); 
  setmotor(1,0);

  delay(1000);   
}

// Sets RGB leds to current RGB value, regardless of what library
void setPixelRGB(int Pixel, byte red, byte green, byte blue) {
  
  // light
  cRGB value;
  value.b = blue; value.g = green; value.r = red; // RGB Value
  LED.set_crgb_at(Pixel, value); // Set value at LED found at index 0
  LED.sync(); // Sends the value to the LED
  
  // Neopixel
//   strip.setPixelColor(Pixel, strip.Color(red, green, blue));
//   neopixel.show();

}