Fall 2019 ModWheels Firmware

Fig. 1: System Flow Diagram

This system flow chart shows the basic process for the firmware; the main loop is always waiting for the next command packet to execute a function.

Fall 2019: ModWheels Firmware

Author/s: Kordell Tan (Project Manager)
Verification:
Approval:

Table of Contents

Discussion

This generation of ModWheels was intended to implement a velocity controller using a programmable PID controller. Due to the extra-added features of the turn blinkers, headlights, speakers, shaft encoder inputs, the use of a TCA9534A I2C I/O expander was needed to accommodate the extra ports needed to handle these devices. While writing the data to I2C was achieved, reading the input (rising edge) of the shaft encoders using the I2C proved to be a challenge as the master-slave train was in constant communication with the LEDs. To access the information of the shaft encoder outputs within the I2C bus, we would have to initiate contact with the I2C each time we expect a pin change to occur. For further generations, we recommend using an I2C expander with multiple accessible addresses for different slave-devices.

Software Code

ModWheels V.I

ModWheels Fall 2019 VI implements drive using the onboard motor driver and turning capabilities using the dedicated servo pins.

/* MW_V1.1
* -Kordell Tan 
* 
* Telemetry and Custom Command Sample Arduino Code
* Telemetry Example: Monitor an extended I/O register value and send to control panel display
* Command Example: Add Robot unique MOVE and SERVO commands
*/

// include C:\Program Files (x86)\Arduino 1_6_5\hardware\arduino\avr\libraries
#include  // instantiated as ArxRobot at end of class header
#include 
#include  // I2C support
#include 

ArxRobot ArxRobot; // instantiated as ArxRobot at end of class header

/*
* Command Example
* Step 1: Assign new command mnemonics ID numbers
* In our example we will be adding 3 custom commands (2 new and one predefined).
* The predefined MOVE command is intercepted and our robot unique code is to be
* run instead. The MOVE command mnemonic ID 0x01 is already defined in Configure.h
* The next two commands are new and assigned to the first two addresses in the
* custom command address space 0x40 - 0x5F.
*/

#define SERVO 0x41
#define SERVO2 0x42
#define STBY 8

Motor motorA; // Create Motor A
Motor motorB; // Create Motor B
Servo servo11; // Create servo object
Servo servo13;

const uint8_t CMD_LIST_SIZE = 2; // we are adding 2 commands (MOVE, SERVO1)

void ServoTest() {
for (int x = 0; x < 1; x++)
{
servo11.attach(11);
servo11.write(90);
delay(100);
servo11.write(70);
delay(100);
servo11.write(110);
delay(100);
servo11.detach(); 
}
servo11.attach(11);
servo11.write(90);
}

/*
* Override MOVE (0x01) Command Example
* A5 05 01 01 80 01 80 A1
*/
void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
Serial.write(cmd); // move command = 0x01
Serial.write(n); // number of param = 4
for (int i=0;i Board.
*/
#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega16U4__)
uint16_t pwm_reading = (uint16_t) OCR4D; // read 8-bit Output Compare Register Timer 4D and cast to 16-bit signed word
motorPWM.sendSensor(pwm_reading);
#else
// Timer/Counter 0 registers set by UNO Bootloader (freq. = 1 Khz)
// Timer/Counter Control Registers TCCR0B = 0x03 (Prescaler of 64) and TCCR0A (Fast PWM, TOP = 0xFF) = 0x03
uint16_t pwm_reading = (uint16_t) OCR0B;
motorPWM.sendSensor(pwm_reading);
#endif
}

Note the above code is able to control the movement of our simple concept version of the F19 ModWheels car. The function ServoTest() simply is a function that only occurs at start-up (during setup) and is done to indicate if the servo is operational. Typically in our tests, if the servo test is unable to run, the robot will not respond if further commands are sent. We have not verified if it is because the pointer is stuck somewhere in that function, or if the inability to pass through the setup portion of the code simply breaks the program.

ModWheels V.II

ModWheels Fall 2019 VII implements differential drive using the onboard motor driver and turning capabilities using the dedicated servo pins, along with I2C GPIO Expander access to trigger headlight and turn blinkers.

Setting Up VII

Here, we create global variables the program will have access to. These variables are our motor speeds and direction and a brake flag. Since the speeds and directions are easy to track, we do this and initialize with these data types to save memory.

/* MW_V2.2 - ArxCruiser
* -Kordell Tan 
* 
* Telemetry and Custom Command Arxterra 3DoT Code
* This file includes the definitions for the Fall 2019 ModWheels: ArxCruiser
* Features:
* + Turn Signaling
* + Horn Simulation
* + Differential Drive
* + D-Pad Control
*/
static uint8_t speedA = 0x00;
static uint8_t speedB = 0x00;
static uint8_t LM = 0x00;
static uint8_t RM = 0x00;
static uint8_t brakes = 0x00;

// include C:\Program Files (x86)\Arduino 1_6_5\hardware\arduino\avr\libraries
#include  // instantiated as ArxRobot at end of class header
#include 
#include  // I2C support

ArxRobot ArxRobot; // instantiated as ArxRobot at end of class header

// Steps for current limiter
uint8_t N = 60;

I2C Read & Write

To access the shaft encoder outputs and assign bit values to our four LEDs, we use an I2C GPIO expander IC. It is a simple process to write data to the register. We define the I2C register address of operation, then we have to configure the pins. After the configuration is complete, we can write whatever data we want to that register. Typically out = 0x01 and in = 0x00. We have to remember to end the communication with the slave device. A delay is forced in this function to ensure data transmission was completed.

/*
 * Setting Up the I2C
 */
#define address      0x38
#define shaft        0x39
#define IN           0x00
#define OUT          0x01

void I2Cwrite(uint8_t config_, uint8_t data){
  Wire.beginTransmission(address);
  Wire.write(config_);
  Wire.write(data);
  Wire.endTransmission();
  delay(50);
}

static uint8_t encoder = 0x00;
// NOTE: No parameters, function will save shaft input to variable 'encoder'
void I2Cread() {
  Wire.beginTransmission(address);
  Wire.write(OUT);
  Wire.requestFrom(address,2);
  encoder = Wire.read();
  Wire.endTransmission();
  delay(50);
}

PWM Sound Signal Sample Code

We use the open source libraries to find this test code that implements sound using PWM. Explanation of the sample code can be found here, as it is open to public access.

#define speakerOut      22
//  timePWMHigh = 1/(2 * frequency) = period / 2
//      note         period    frequency
#define  c            3830    // 261 Hz 
#define  d            3400    // 294 Hz 
#define  e            3038    // 329 Hz 
#define  f            2864    // 349 Hz 
#define  g            2550    // 392 Hz 
#define  a            2272    // 440 Hz 
#define  b            2028    // 493 Hz 
#define  C            1912    // 523 Hz
// Rest
#define  R               0

int melody[] = {  C,  b,  g,  C,  b,   e,  R,  C,  c,  g, a, C };
int beats[]  = { 16, 16, 16,  8,  8,  16, 32, 16, 16, 16, 8, 8 }; 
int MAX_COUNT = sizeof(melody) / 2; // Melody length, for looping.
  
// Set overall tempo
  long tempo = 10000;
  
// Set length of pause between notes
  int pause = 1000;
  
// Loop variable to increase Rest length
  int rest_count = 100;
  int tone_      =   0;
  int beat       =   0;
  long duration  =   0;

void song()
{
  long elapsed_time = 0;
  if (tone_ > 0) {                    // if this isn't a Rest beat, while the tone has 
                                      //  played less long than 'duration', pulse speaker HIGH and LOW
    while (elapsed_time < duration) {

      digitalWrite(speakerOut,HIGH);
      delayMicroseconds(tone_ / 2);

      // DOWN
      digitalWrite(speakerOut, LOW);
      delayMicroseconds(tone_ / 2);   // use delayMicroseconds because period is measured in microseconds

      // Keep track of how long we pulsed
      elapsed_time += (tone_);
    } 
  }
  else {                                    // Rest beat; loop times delay
    for (int j = 0; j < rest_count; j++) {
      delayMicroseconds(duration);  
    }                                
  }
}

void play()
{
  for (int i=0; i

Turn Signals and Headlights

We define our conditions for our turn blinkers and headlights because the I2C can only be accessed with one string of data at a time. So our conditions would be (turn blinkers, headlights, turn blinkers and headlights, off).

// Define DPINS for Custom Shield
#define lightsOff  0x00
#define leftBlink  0x01
#define rightBlink 0x08
#define headlight  0x06
#define hazard     0x09
#define Llights    0x07
#define Rlights    0x0E
#define Hlights    0x0F

// Speaker Pin
#define horn         22

Motor motorA;       // Create Motor A
Motor motorB;       // Create Motor B
Servo servo11;      // Create servo object

const uint8_t CMD_LIST_SIZE = 5;   // we are adding 4 commands (MOVE, SERVO1, TURNSIGNAL, BEEP, HEADLIGHTS, BRAKE)

void turnsig (uint8_t cmd, uint8_t param[]){
  checkturn = param[0];
    // Update Turn Signal Blinkers
} // turnsig

void blinker()
{
    switch (checkturn)
  {
    case 0x00:
    if (checklight == 0x01) {
      I2Cwrite(OUT,headlight);
    }
    if (checklight == 0x00) {
      I2Cwrite(OUT,lightsOff);
    }
    break;
    case 0x01:
    // Turning Right
    if (checklight == 0x01) {
      I2Cwrite(OUT,Rlights);
      delay(500);
      I2Cwrite(OUT,headlight);
      delay(500);
    }
    if (checklight == 0x00) {
      I2Cwrite(OUT,rightBlink);
      delay(500);
      I2Cwrite(OUT,lightsOff);
      delay(500);
    }
    break;
    case 0x02:
    // Turning Left
    if (checklight == 0x01) {
      I2Cwrite(OUT,Llights);
      delay(500);
      I2Cwrite(OUT,headlight);
      delay(500);
    }
    if (checklight == 0x00) {
      I2Cwrite(OUT,leftBlink);
      delay(500);
      I2Cwrite(OUT,lightsOff);
      delay(500);
    }
    break;
    case 0x03:
    // Hazards ON
    if (checklight == 0x01) {
      I2Cwrite(OUT,Hlights);
      delay(500);
      I2Cwrite(OUT,headlight);
      delay(500);
    }
    if (checklight == 0x00) {
      I2Cwrite(OUT,hazard);
      delay(500);
      I2Cwrite(OUT,lightsOff);
      delay(500);
    }
    break;
  }
  return;
}

void headlights (uint8_t cmd, uint8_t param[])
{
  checklight = param[0];
  // Update Headlights
}

void beep (uint8_t cmd, uint8_t param[])
{
  int i = param[0];
}


Note that the function "blinker()" will be in the main loop and will check condition of the flags and update the LEDs per clock cycle.

Move Override

The movehandler() function compares all four parameters received by the packet sent by the mobile application. It can be done in several different ways. Here, we compare directions first to determine overall speed and range.

// Define Speed Parameters and Range
uint8_t maxspd = 231;
uint8_t minspd = 43;
void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
  LM = param[0];               // Check direction of motors
  RM = param[2];
                               // move command = 0x01
                               // number of param = 4
                               // param = L_dir L_spd R_dir R_spd
    // AUTOMATIC BRAKE
    if (brakes == 0x01) {
      if (LM == 0x04) {
        for (speedA;speedA > 0x00; speedA--) {
          motorA.go(LM,speedA);
          delay(100);
          break;
        }
        for (speedB;speedB > 0x00; speedB--) {
          motorB.go(RM,speedB);
          delay(100);
          break;
        }
      }
    }
//    /*
//     * D-PAD Control
//     *    AND Logic compares direction of both motors ***(RM is reverse polarity)***
//     *      +  01,01 = FORWARD 
//     *      +  02,02 = BACK
//     *      +  01,02 = LEFT
//     *      +  02,01 = RIGHT
//     */
     if (LM == 0x02 & RM == 0x02)   // Turn Right
     {
      // Differential Drive (LEFT MOTOR > RIGHT MOTOR)
        servo11.write(90);
        if (speedA > minspd) {
          speedA = speedA - 0x15;
          motorA.go(0x01,speedA);
          delay(50);
        }
        if (speedB < maxspd) {
          speedB = speedB + 0x15;
          motorB.go(0x02,speedB);
          delay(50);
        }
     }
     if (LM == 0x01 & RM == 0x01)   // Turn Left
     {
      // Differential Drive (RIGHT MOTOR > LEFT MOTOR)
        servo11.write(110);
        if (speedB > minspd) {
          speedB = speedB - 0x15;
          motorB.go(0x02,speedB);
          delay(50);
        }
        if (speedA < maxspd) {
          speedA = speedA + 0x15;
          motorA.go(0x01,speedA);
          delay(50);
        }
     }
/*
 * FORWARD/REVERSE DRIVE
 */
     if (LM == 0x01 & RM == 0x02 || LM == 0x02 & RM == 0x01)
     {
      /*
       * Gradual increase/decrease in speed in the forward/reverse drive modes
       * Maximum PWM amplitude is designated 231 (6V motors)
       * Minimum PWM amplitude is 43
       * increments are in steps of 15
       */
      if (LM == 0x01) {
        if (speedA < maxspd && speedA < param[1]) {
        speedA = speedA + 0x15;
        delay(250);
        }
        if  (speedB < maxspd & speedB < param[3]) {
        speedB = speedB + 0x15;
        delay(250);
        }
      }
      else if (LM == 0x02) {
        if (speedA > minspd && speedA > param[1]) {
        speedA = speedA - 0x15;
        delay(250);
        }
        if (speedB > minspd && speedB > param[3]) {
        speedB = speedB - 0x15;
        delay(250);
        }
      }
        motorA.go(LM,speedA);
        motorB.go(RM,speedB);
        servo11.write(100);                   // Reorient servo to the middle
     }
}  // moveHandler

void brake (uint8_t cmd, uint8_t param[])
{
  brakes = param[0];
}

References/Resources