Fall 2019 ModWheels Firmware
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]; }