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

The state sequence is intended to make some sequences to implement the desired manoeuvres.

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:
     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:
     // 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:
     if (time_sec > endTime)
     { // change to next values
       desiredValue = 3;
       endTime = time_sec + 0.50;
       state = 12;
     }
     break;
   case 12:
     if (time_sec > endTime)
     { // change to next values
       desiredValue = 6;
       endTime = time_sec + 0.5;
       state = 100;
     }
     break;
   case 100:
     // 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;
 }
}

When 'state' is '0' and the start condition is fulfilled (the start button is pressed), 'state' is changed to 10. This state also stops the display update. Display update takes a relatively long time, and if the sample time is short (<2ms), there will be samples that are not run at the desired time.

When the state is 10, nothing happens until 500us has passed. Then, the desired value for the controller is set, and the state is changed to 20.

States 20 and 30 change the desired value after a given time.

State 98 allows the values for the stopping motors to be included in the mission.

State 99 stops the motors and allows the display to be updated. In this version, a printout of the actual measured sample time (estimated by the encoder module) is included.

Control and print results over USB

/**
 * make the control */
void controlUpdate()
{ // do control during a mission only.
 if (state > 0)
 { // running a sequence, so do control
   float ref = desiredValue;
   //
   // set actuators
   motor.motorVoltage[0] = -ref; // left motor
   motor.motorVoltage[1] = ref;  // right motor
   //
   if (true)
   { // see the result
     Serial.print(float(micros() - startTime*1000)/1000.0);
     Serial.print(" ");
     Serial.print(ref);
     Serial.print(" ");
     Serial.print(-encoder.motorVelocity[0]); // radians/second
     Serial.print(" ");
     Serial.print(encoder.motorVelocity[1]);
     Serial.println();
   }
 }
}

The function runs if the mission state 'state' is not zero (to avoid spamming the USB when nothing happens).

This function takes the desiredValue from the test sequence and sets this value as the motor voltage.

The rest is printing values to be sent over the USB connection.

The distance travelled for each wheel is found by multiplying the motor velocity (in radians per second) with sample time, the wheel radius (0.03m) and reduced by the gearing (9.28).