Idea on Guidance for HugoBot in Arduino (semi-pseudo code)

SampleGuidance.ino

#include "MeMegaPi.h"
#include "Arduino.h"
#include "SoftwareSerial.h"


// Define the pins/outputs that map to the individual motors
#define WHEEL_LF = PORT2B   // Set to the pin for LEFT FRONT wheel
#define WHEEL_RF = PORT1A   // Set to the pin for RIGHT FRONT wheel
#define WHEEL_LR = PORT2A   // Set to the pin for LEFT REAR wheel
#define WHEEL_RR = PORT1B   // Set to the pin for RIGHT REAR wheel


// Multipliers for direction "correction"
#define FORWARD    1
#define REVERSE   (int8_t)-1
#define STOP       0


// The 9 (including stopped) the bot can move, from the attributions 
// (https://github.com/pulcher/HugoBot/blob/main/research/attributions.md)
// page
#define DIRECTION_NW    0
#define DIRECTION_N     1
#define DIRECTION_NE    2
#define DIRECTION_W     3
#define DIRECTION_NONE  4
#define DIRECTION_E     5
#define DIRECTION_SW    6
#define DIRECTION_S     7
#define DIRECTION_SE    8

#define DIRECTION_ROTL  9
#define DIRECTION_ROTN  10
#define DIRECTION_ROTL  11

#define WHEELS_FRONT 0    // Lookup index for the FRONT wheels, in the "directions" array
#define WHEELS_READ  1    // Lookup index for the REAR wheels, in the "directions" array


#define WHEELS_LEFT  0    // Lookup index for the LEFT wheel in the inner "directions" array
#define WHEELS_RIGHT 1    // Lookup index for the READ wheel in the inner "directions" array


const int8_t Front_Wheels[2]    = {FORWARD, REVERSE};
const int8_t Rear_Wheels[2]     = {FORWARD, REVERSE};
const int8_t* All_Wheels[2]     = {Front_Wheels, Rear_Wheels};

const int8_t Directions [12][2][2] = {
                                    { {STOP, FORWARD},     {FORWARD, STOP}},       {{FORWARD, FORWARD},  {FORWARD, FORWARD}},    {{FORWARD, STOP}, {STOP, FORWARD}       },
                                    { {REVERSE, FORWARD},  {FORWARD, REVERSE}},    {{STOP, STOP},        {STOP, STOP}},          {{FORWARD, REVERSE}, {REVERSE, FORWARD} },
                                    { {REVERSE, STOP},  {STOP, REVERSE}},          {{REVERSE, REVERSE},  {REVERSE, REVERSE}},    {{STOP, REVERSE}, {STOP, REVERSE} },
                                    { {REVERSE, FORWARD},  {REVERSE, FORWARD}},    {{STOP, STOP},        {STOP, STOP}},          {{FORWARD, REVERSE}, {FORWARD, REVERSE} }
                                 };


MeMegaPiDCMotor motors[4] = { WHEEL_LF, WHEEL_RF, WHEEL_LR, WHEEL_RR };


void moveBot(uint8_t botDirection, int8_t speed) {
   byte direction[2,2] = Directions[botDirection];
   setWheelSpeed(WHEEL_LF, direction[WHEELS_FRONT][WHEELS_LEFT]   * All_Wheels[WHEELS_FRONT][WHEELS_LEFT]  * speed);
   setWheelSpeed(WHEEL_RF, direction[WHEELS_FRONT][WHEELS_RIGHT]  * All_Wheels[WHEELS_FRONT][WHEELS_RIGHT] * speed);
   setWheelSpeed(WHEEL_LR, direction[WHEELS_REAR][WHEELS_LEFT]    * All_Wheels[WHEELS_REAR][WHEELS_LEFT]   * speed);
   setWheelSpeed(WHEEL_RR, direction[WHEELS_REAR][WHEELS_RIGHT]   * All_Wheels[WHEELS_REAR][WHEELS_RIGHT]  * speed);
}


void setWheelSpeed(uint8_t wheelIndex, int8_t speed) {
  Serial.print("Setting wheel ");
  Serial.print(wheelIndex);
  Serial.print(" to speed ");
  Serial.println(speed);
}