     } 
   }
 
-  // Format a basic speed command.  Bytes 3,4 are MSB,LSB of signed right speed and bytes 5,6 are signed left speed
-
-  // Pack in the 2 speed bytes
-  if (rightSpeed < 0) {
-        speedCmd[3] = 0xff;
-        speedCmd[4] = 0xff - ((-1 * rightSpeed) & 0xff);
-  } else {
-        speedCmd[3] = 0;
-        speedCmd[4] = rightSpeed & 0xff;
-  }
-  if (leftSpeed < 0) {
-        speedCmd[5] = 0xff;
-        speedCmd[6] = 0xff - ((-1 * leftSpeed) & 0xff);
-  } else {
-        speedCmd[5] = 0;
-        speedCmd[6] = leftSpeed & 0xff;
-  }
-
-  // The start byte is 0x7E so the cmdPacket where we calculate checksum does not include that
-  cmdPacketLen = 6;
-  speedCmd[cmdPacketLen+1] = calcPacketCksum( &speedCmd[1], cmdPacketLen);
-  msgXmitLen = cmdPacketLen + 2;  // add in start byte and checksum bytes for total xmit length
-
-  if (debugMode == 1) {
-    loopTimeMs = 1000;
-    if (joystickEnable == 0) {
+  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
+
+  // Set motor speeds if enabled
+  // Write out the speed command if transmit is enabled
+  if ((g_xmitEnable != 0) && (g_debugMode == 0)) {
+     setMotorSpeeds(leftSpeed, rightSpeed);
+  }
+  
+  if (g_debugMode == 1) {
+    g_loopTimeMs = 1000;
+    if (g_joystickEnable == 0) {
       Serial.print("MotorSpeeds:  Left= "); Serial.print(leftSpeed); Serial.print("  Right= "); Serial.println(rightSpeed);
     } else {
       Serial.print("JoystickSpeeds:  Left= "); Serial.print(leftSpeed); Serial.print("  Right= "); Serial.println(rightSpeed);
     }
-    Serial.print("Msg: ");
-    for (int i = 0; i<msgXmitLen ; i++) {
-      Serial.print("  "); Serial.print( speedCmd[i]);
-    }
-    Serial.println("\n");
-  }
-  
-  // Write out the speed command if transmit is enabled
-  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
-  if ((xmitEnable != 0) && (debugMode == 0)) {
-     Serial.write(speedCmd, msgXmitLen);
   }
   digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
   
-  delay(loopTimeMs);                 // remain off this long
-}
+  delay(g_loopTimeMs);                 // remain off this long
+}

/*
  MCB Controller That Runs On The Arduino Platform for debug or very very simple controllers

  A basic MCB speed control for the Ubiquity Robotics Magni Robot is implemented
  This code is meant for the Arduino Nano model but because of how IDE works can easily be used
  on other processors as supported by the Arduino IDE board support packages. .

  This is meant as a debug tool for board bringup as well as example code showing how to send 
  Magni commands with the way we do checksum and transmission over serial.

  User needs to power up his Arduino Nano from a usb power source or directly from Magni P508  GND and VCC
  The Arduino 5V level TX pin goes to the P508 5V RX pin and Arduino Ground goes to P508 ground.
  Optionally you can power the Arduino all from P508 because there is 5V VCC on P508 as well to go to Nano +5 line.
  (Note that to do that you must tie MCB Vcc on P508 to Arduino 5V power because the spec for VIN is for 6V or more)

  Usage Instructions
  - Connect two 1k to 20k linear taper pots with each of the ends on GND and Vcc.  
    Center tap of one pot to Arduino A1 and the center tap of the other pot to Arduino A2.
  - Default with no jumpers is for each pot to separately control a single wheel.
  - Optionally ground the ENABLE_JOYSTICK pin to interprit two pots as a simple joystick
  - Optionally a normaly closed switch from ground to ENABLE_SPEED_PIN for switch that must close to run motors
    When the ENABLE_SPEED_PIN is floating motors are enabled. When it goes to ground motors stop

  Author:  mj@ubiquityrobotics.com
  Date:    20210103
  
*/

#define ENABLE_SPEED_PIN 3   // Release from ground to enable speed messages (thus use NC switch to use this feature)
#define ENABLE_JOYSTICK  2   // Pull to ground to enable interpritation of pots as a joystick
#define SPEED_POT_RIGHT  A1  // Motor speed control for right wheel
#define SPEED_POT_LEFT   A2  // Motor speed control for left  wheel

int debugMode = 0;        // set to 1 to use serial monitor but normally 0 for real speed commands out

long loopTimeMs = 50;     // Need to keep repeating speeds or deadman timer kicks in on MCB
long pulseOnTimeMs;       // On time in millisec
long pulseOffTimeMs;      // Off time in millisec

int  leftSpeed    = 0;
int  rightSpeed   = 0;
int  linearSpeed  = 0;
int  rotateSpeed  = 0;

int  xmitEnable     = 0; // When non-zero we allow speed messages to be continuously sent. Set to -1 to now use this feature
int  joystickEnable = 0; // When positive (input pin ground) this interprites pots as from a joystick. Set to -1 for never joystick

// Format a basic speed command.  Bytes 3,4 are MSB,LSB of signed right speed and bytes 5,6 are signed left speed
byte speedCmd[]={0x7e, 0x3b, 0x2a, 0, 0, 0, 0, 0};
int  cmdPacketLen = 8;
int  msgXmitLen   = 0;


// Calculate a simple single byte checksum from a byte array of given length

// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN,      OUTPUT);
  pinMode(VIRT_GND_1,       OUTPUT);
  pinMode(RELAY_1_DRIVE,    OUTPUT);
  pinMode(VIRT_VCC_1,       OUTPUT);
  
  pinMode(ENABLE_SPEED_PIN, INPUT_PULLUP);
  pinMode(ENABLE_JOYSTICK,  INPUT_PULLUP);
  pinMode(SPEED_POT_RIGHT,  INPUT);
  pinMode(SPEED_POT_LEFT,   INPUT);
  pinMode(VAR_AUTO_RUN,     INPUT_PULLUP);
  pinMode(FIXED_AUTO_RUN,   INPUT_PULLUP);

  Serial.begin(38400);     // Set to speed for MCB control

  digitalWrite(RELAY_1_DRIVE, HIGH);
  digitalWrite(VIRT_VCC_1,    HIGH);
  digitalWrite(VIRT_GND_1,    LOW);
}

// Send out a speed command for both motors
void setMotorSpeeds(int leftSpeed, int rightSpeed)  {
  // Format a basic speed command.  Bytes 3,4 are MSB,LSB of signed right speed and bytes 5,6 are signed left speed
  byte speedCmd[]={0x7e, 0x3b, 0x2a, 0, 0, 0, 0, 0};
  int  cmdPacketLen = 8;
  int  msgXmitLen   = 0;
  
  // Bytes 3,4 are MSB,LSB of signed right speed and bytes 5,6 are signed left speed
  // Pack in the 2 speed bytes
  if (rightSpeed < 0) {
        speedCmd[3] = 0xff;
        speedCmd[4] = 0xff - ((-1 * rightSpeed) & 0xff);
  } else {
        speedCmd[3] = 0;
        speedCmd[4] = rightSpeed & 0xff;
  }
  if (leftSpeed < 0) {
        speedCmd[5] = 0xff;
        speedCmd[6] = 0xff - ((-1 * leftSpeed) & 0xff);
  } else {
        speedCmd[5] = 0;
        speedCmd[6] = leftSpeed & 0xff;
  }

  // The start byte is 0x7E so the cmdPacket where we calculate checksum does not include that
  cmdPacketLen = 6;
  speedCmd[cmdPacketLen+1] = calcPacketCksum( &speedCmd[1], cmdPacketLen);
  msgXmitLen = cmdPacketLen + 2;  // add in start byte and checksum bytes for total xmit length
  
  // Write out the speed command over serial port
  Serial.write(speedCmd, msgXmitLen);
}


int  leftSpeed    = 0;
int  rightSpeed   = 0;
int  linearSpeed  = 0;
int  rotateSpeed  = 0;
int  reversePeriodMs = 1000;
int  autoDirection = 1;   
int  periodMs     = 30000;  // Make it big to start
  
// the loop function runs over and over again forever
void loop() {

  // Setup mode based on pins we see in a given state. If this was set to -1 always enable motors
  // The idea here is that a normally closed deadman switch can be used so motors stop if switch is not pressed
  if (g_xmitEnable >= 0) {
    g_xmitEnable = digitalRead(ENABLE_SPEED_PIN);
  }

  g_varAutoRun   = digitalRead(VAR_AUTO_RUN);     // See if we are to do variable auto-run mode
  g_fixedAutoRun = digitalRead(FIXED_AUTO_RUN);   // See if we are to to fixed speed and period auto-run mode

  // Allow two pots to be interprited as a simple joystick control.  Default is each pot controls one motor.
  if (g_joystickEnable >= 0) {
    g_joystickEnable = 0;
    if (digitalRead(ENABLE_JOYSTICK) == 0) {
      g_joystickEnable = 1;
    }
  }

  // Determine motor right and left speed based on mode we are in at the time
  if ((g_varAutoRun == 0) || (g_fixedAutoRun == 0)) {
    if (g_fixedAutoRun == 0)  {   // Used constants so no pot is required
      // Fixed default test where very simple wiring does auto reversing default speeds and reversal time
      leftSpeed = 20;
      reversePeriodMs = 4000;
    } else {
      // Left pot is speed and right pot is time interval between speed reversals
      reversePeriodMs = ((long)analogRead(SPEED_POT_RIGHT) * 10) + 500;
      leftSpeed  = ((long)analogRead(SPEED_POT_LEFT) - 512)  / 8;     // allow signed 0-64
    }
    
    // Reversal time has a minimum time and then pot setting adds to that up to 10 sec more
    
    if (periodMs > reversePeriodMs) {
      autoDirection = autoDirection * -1;    // Reverse direction
      periodMs = 0;                          // reset clock for next period 
    }
    periodMs += g_loopTimeMs;

    // Left pot is signed speed for when autoDirection is 1 and gets reversed when autoDirection = -1
    leftSpeed = leftSpeed * autoDirection;  
    rightSpeed = leftSpeed;    // In autoRun mode both speeds are the same

    // We drive an optional reed relay that alternates on to off as an extra feature
    if (autoDirection > 0) {
      digitalWrite(RELAY_1_DRIVE, LOW);
    } else {
      digitalWrite(RELAY_1_DRIVE, HIGH);
    }
    
  } else if (g_joystickEnable <= 0) {
    // Directly determine speeds from two pots in joystick mode
    leftSpeed  = ((long)analogRead(SPEED_POT_LEFT) - 512)  / 8;     // allow signed 0-64
    rightSpeed = ((long)analogRead(SPEED_POT_RIGHT) - 512) / 8;     // allow signed 0-64
    
  } else {
    // Joystick mode: Interprit pots as one is forward/reverse and the other is bias to right or left (rotation)
    linearSpeed  = ((long)analogRead(SPEED_POT_LEFT) - 512)  / 8;     // allow signed 0-64
    rotateSpeed  = ((long)analogRead(SPEED_POT_RIGHT) - 512) * 2;     // This is near +- 1024

