Basebot main code

Back to Basebot

Main code

The main code is in basebot_6.ino (sometimes renamed to basebot_6.cpp).

The code is based on the Arduino environment. Here, the normal C (and C++) main() function is hidden. However, setup() is called first, followed by an endless loop that calls loop().

The Basebot API is in the "src" sub-directory.

Setup

The setup() function is like this and initializes all support modules in the Basebot API:

void setup()   // INITIALIZATION
{ // to be able to print to USB interface
 Serial.begin(12000000);
 // start timing
 sampleTimer.begin(timerInterrupt, sampleTimeUs);
 // Initialise sensors, safety and display (in the basebot API)
 robot.setup(); // alive LED, display and battery
 motor.setup(); // motor voltage
 encoder.setup(); // motor encoders to velocity
 encoder.pulsPerRev = 48;
 imu2.setup(); // gyro and accelerometer
 motor.setPWMfrq(80000);
 usb.setup();
}

Timing

An interrupt controls timing.

// Sample time can not go lower than 300us
const uint32_t sampleTimeUs = 1000; // desired sample time in us
bool isSampleTime = false;
float time_sec = 0;

void timerInterrupt()
{
 isSampleTime = true;
 time_sec += sampleTimeUs / 1e6;
}

Loop

The loop() function waits for the "isSamplingTime" flag to ensure a constant sample time.

Then all sensor data are updated (by the .tick() calls).

The "sequence" decides where to go when in a sequence of states.

The "control" function translates the desired sequence to motor voltage.

The motor voltage is then implemented by the "motor.tick()" function.

The essential data values are then added to the RAM-based log.

If it is not sampling time, then potential commands from the USB connection are serviced.

void loop ( void )
{ // init sample time
 while ( true )
 { // main loop
   // loop until time for next sample
   if (isSampleTime) // start of new control cycle
   { // time for next tick
     isSampleTime = false;
     // read sensors
     imu2.tick();
     encoder.tick();
     // updatePose();
     // advance sequence (default is wait for start button)
     sequenceTwoSteps();
     //
     if (state > 0)
     { // Only if started
       // Calculate new motor voltage
       controlUpdate();
     }
     // give value to actuators
     motor.tick();
     // save relevant values
     updateLog();
     // support functions
     robot.tick();   // measure battery voltage etc.
     display.tick(); // update O-LED display
   }
   usb.tick(); // listen to incoming from USB
 }
}

State sequence

A state sequence is needed to implement a desired manoeuvre.

Global variables

int state = 0;               // actual sequence state
float endTime = 0;           // for current state
float desiredValue = 0;      // desired (reference) value send to controller

void sequenceTwoSteps()
{ // this function is called at every sample time
 // and should never wait in a loop.
 bool button;
 // this is a state machine
 // state 0: wait for start button press
 switch (state)
 { // run mission, initial value
   case 0: // State 0 is just inactive, waiting for the start signal.
     button = digitalReadFast(PIN_START_BUTTON);
     if (button or robot.missionStart)
     { //  Starting a sequence
       start();
       // Prepare next state
       desiredValue = 0; // reference value to the controller
       // to start the log with no velocity
       endTime = time_sec + 0.020; // new state to end after 20ms
       state = 10;
     }
     break;
   case 10: // Waiting for first step.
     // test if this state is finished
     if (time_sec > endTime)
     { // change to next values
       desiredValue = 1.5;
       endTime = time_sec + 0.48 ; // ~ 0.5 second
       state = 11;
     }
     break;
   case 11: //  First step
     if (time_sec > endTime)
     { // change to next values
       desiredValue = 3;
       endTime = time_sec + 0.50;
       state = 12;
     }
     break;
   case 12: //  Second step.
     if (time_sec > endTime)
     { // change to next values
       desiredValue = 6;
       endTime = time_sec + 0.5;
       state = 100;
     }
     break;
   case 100: //  Maintain third level.
     // stop after end time
     if (time_sec > endTime)
     { // stop, but continue logging for a while (0.3 sec)
       stop(0.3); // actually sets state to 999 (i.e. default:)
     }
     break;
   default:
     // this state is needed to enable a new start
     if (time_sec  > endTime)
       finished();
     break;
 }
}

Control

The control function should implement the (PID) controller calculations.

The desired value (forward velocity) can here be compared with actual velocity and used to set a motor voltage that will implement the desired behaviour.

This can later be extended also to implement a desired turn rate.

void controlUpdate()
{ // do control during a mission only.
 //
 // Measured wheel velocity (positive is forward)
 // float velLeft = -encoder.motorVelocity[0];
 // float velRight = encoder.motorVelocity[1];
 //
 // Desired velocity
 float velRef = desiredValue;
 //
 // For a start, the desiredValue
 // is just used as motor voltage
 //
 // Left motor voltage (-9V to +9V)
 motor.motorVoltage[0] = -velRef;
 // Right motor voltage (-9V to +9V)
 motor.motorVoltage[1] = velRef;
}

Update log

To document the effect of the controller, data is needed.

An array of data structures are implemented with some of the desired data.

The data structure is initially set up as follows.

typedef struct
{
 float time; // in seconds
 int state;  // sequence state
 float desiredValue;
 float motorVel[2]; // left, right
 float motorVolt[2];// left, right
 int32_t encoder[2];// Encoder count value on each motor
 float motorCurrent[2]; // rather poor measurement of motor current
 float battery; // Battery voltage
} LogData;

A large portion of memory is allocated for the log:

// allocate memory for data log in memory block 2 available only through malloc/new
// (specific for this micro processor, a Teensy 4)
const int logsMax = 400000 / sizeof(LogData); // allocates 400kBytes
LogData * logs = (LogData*)malloc(logsMax * sizeof(LogData));
int logsCnt = 0; // next log entry to use / i.e. number of used log entries

A function adds data to the log.

void updateLog()
{ // step response mode
 if (state > 0 and logsCnt < logsMax)
 {
   logs[logsCnt].time = time_sec;
   logs[logsCnt].state = state;
   logs[logsCnt].desiredValue = desiredValue;
   logs[logsCnt].motorVolt[0] = -motor.motorVoltage[0]; // The left motor runs backwards, therefore the sign.
   logs[logsCnt].motorVolt[1] = motor.motorVoltage[1];
   logs[logsCnt].encoder[0] = -encoder.encoder[0];
   logs[logsCnt].encoder[1] = encoder.encoder[1];
   logs[logsCnt].motorVel[0] = -encoder.motorVelocity[0];
   logs[logsCnt].motorVel[1] = encoder.motorVelocity[1];
   logs[logsCnt].battery = robot.batteryVoltage;
   // the last part is offset of current measurement (in Amps)
   // take the initial current readings and add or subtract as needed
   // these values fits Tania: (+0.260A, -0.275A)
   logs[logsCnt].motorCurrent[0] = -(analogRead(A1) - 2024) * 0.0045 + 0.260;
   logs[logsCnt].motorCurrent[1] =  (analogRead(A0) - 2024) * 0.0045 - 0.275;
   logsCnt++;
 }
}

The motor current is just read from the sensor pins. This is a 12-bit (4096) A/D converter, and half that value (2048) is close to zero current. Each LSB is about 4.5mA. An offset current is added at the end. This value varies for each robot and must be calibrated.

Once the log is filled, it needs to be transferred to the PC.

All values are just printed to the USB connection, and preceded by a textual explanation of the data.

void printLog()
{
 LogData * d = logs;
 Serial.print("% Basebot log for ");
 Serial.println(robot.getRobotName());
 // hard-coded settings
 Serial.print("% Sample time ");  Serial.print(sampleTimeUs);
 Serial.print(" us, Gear ");  Serial.print(gear);
 Serial.print(", Wheels ");  Serial.print(wheelRadius);
 Serial.print(" m, PPR ");  Serial.println(encoder.pulsPerRev);
 Serial.print("% maximum log samples ");  Serial.print(logsMax);
 Serial.print(" each sized ");  Serial.print(sizeof(LogData)); Serial.println(" bytes");
 // Columns
 Serial.println("% 1 time (sec)");
 Serial.println("% 2 state");
 Serial.println("% 3   Desired value");
 Serial.println("% 4-5 motor voltage (left,right) (V)");
 Serial.println("% 6-7 Encoder count (left, right)");
 Serial.println("% 8-9 motor vel (left, right) (rad/s)");
 Serial.println("% 10  Battery voltage (V)");
 Serial.println("% 11-12 Motor current (left, right) (A)");
 for (int i = 0; i < logsCnt; i++)
 {
   // Serial.print(" ");
   Serial.print(d->time,4);    // 1
   Serial.print(" ");    Serial.print(d->state);          // 2
   Serial.print(" ");    Serial.print(d->desiredValue,2); // 3
   Serial.print(" ");    Serial.print(d->motorVolt[0],2); // 4 (V)
   Serial.print(" ");    Serial.print(d->motorVolt[1],2); // 5 (V)
   Serial.print(" ");    Serial.print(d->encoder[0]);     // 6
   Serial.print(" ");    Serial.print(d->encoder[1]);     // 7
   Serial.print(" ");    Serial.print(d->motorVel[0],2);  // 8 (rad/s)
   Serial.print(" ");    Serial.print(d->motorVel[1],2);  // 9 (rad/s)
   Serial.print(" ");    Serial.print(d->battery,2);      // 10 (V)
   Serial.print(" ");    Serial.print(d->motorCurrent[0],3);  // 11 (A)
   Serial.print(" ");    Serial.print(d->motorCurrent[1],3);  // 12 (A)
   Serial.println(" ");
   //delay(1);
   d++;
 }
 Serial.print("% send ");
 Serial.print(logsCnt);
 Serial.println(" log lines");
}

Support functions

A few other support functions are added to keep the code clearer.

The start function empties the log and resets what is needed for a fresh start. Further, the display is disabled to maintain a constant sample rate.

void start()
{ // Start timing
 // reset log and encoders
 logsCnt = 0;
 // change motor PWM frequency for sampling time test
 // should not be above 100000 (100kHz), default is 80kHz.
 motor.setPWMfrq(80000);
 time_sec = 0;
 encoder.encoder[0] = 0; // left motor encoder
 encoder.encoder[1] = 0; // right motor encoder
 // Update of display takes too long for fast sampling
 // So, disable during sequence.
 display.useDisplay = false;
}

The stop function sets the desired velocity to 0, and extends the sequence to make the spin-down part of the log. The spin-down time is set to 0.3 seconds.

void stop(float after)
{ // stop, but allow continued logging for a while
 state = 999;
 desiredValue = 0;
 // end log after an additional short time
 endTime = time_sec + after;
}

The finished function prepares for the next start and re-enables the display. The function further prints the log; this is handy if the USB cable is connected during the run.

void finished()
{ // prepare for a new start
 // stop motors
 motor.motorVoltage[0] = 0; // left motor
 motor.motorVoltage[1] = 0;  // right motor
 // set default PWM frequency
 motor.setPWMfrq(80000);
 // start updating display
 display.useDisplay = true;
 // wait for next button press
 state = 0;
 printLog();
}