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-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:

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;
}