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();

}
facilities/hte/hte_prototypes/brickbattlebots/code.txt · Last modified: 2019/02/19 15:20 by Andrei Maberley
CC Attribution-Share Alike 4.0 International Except where otherwise noted, content on this wiki is licensed under the following license: CC Attribution-Share Alike 4.0 International

We acknowledge Aboriginal and Torres Strait Islander peoples and their continuing connection to land and as custodians of stories for millennia. We are inspired by this tradition in our work to share and preserve Queensland's memory for future generations.