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. But setup() is called first, followed by an endless loop that calles loop().

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);
  // initialize sensors, safety and display
  robot.setup(); // alive LED, display and battery
  motor.setup(); // motor voltage
  encoder.setup(); // motor encoders to velocity
  imu2.setup(); // gyro and accelerometer - include tilt angle
}

Loop

The loop() function:

void loop ( void )
{ // init sample time
 uint32_t nextSample = 0;
 while ( true )
 { // main loop
   // get time since start in microseconds
   uint32_t us = micros();
   // loop until time for next sample
   if (us > nextSample) // start of new control cycle
   { // advance time for next sample
     nextSample += sampleTimeUs;
     // read sensors
     imu2.tick();
     encoder.tick();
     // advance test sequence (default is wait for start button)
     testSequence();
     // make control actions
     controlUpdate();
     // give value to actuators
     motor.tick();
     // support functions
     robot.tick(); // measure battery voltage etc.
     display.tick(); // update O-LED display
   }
   usb.tick(); // listen to incoming from USB
 }
}

The loop (while (true)) runs as fast as it can, and if it is not time for a new sample, then the USB connection is serviced (usb.tick()).

When the sample time has passed, the next sample time is set up. Both use the standard Arduino call micros(), which returns the number of microseconds since boot.

At every sample, the sensors are handled first (imu.tick() and encoder.tick()); these will get new data from the sensor, and the encoder will estimate the motor velocity.

Next is testSequenct(), which is intended as a state machine that changes what the robot should do, as described below. The default is to wait for the start button to be pressed.

All data for the control effort is now available (controlUpdate()). The default controller uses the desired velocity as motor voltage, ignoring the estimated velocity. This is a feed-forward-only controller.

The control effort sets the motor voltages and the motor.tick() implements this as a PWM to the motor driver.

In the end, other service functions are called (robot.tick() and display.tick()), which (mostly) check the battery voltage and update the display.

State sequence

The state sequence is intended to make some small sequences usable for performance analysis and test.

/**
 * Global variables for test sequence */
int state = 0;               // current state
uint32_t startTime = 0;      // time, when 'start' was pressed (in milliseconds)
float desiredValue = 0;      // desired (reference) value sent to the controller.
uint32_t sampleTimeUs = 500; // sample time for the whole system (in microseconds)

The sample time (in microseconds) should not be shorter than about 300 us, as this is what the controller can handle and still send sample results over USB.

/**
 * Test sequence,
 * that is, what should happen when */
void testSequence()
{ // this function is called at every sample time
 // and should never wait in a loop.
 // Update variables as needed and return.
 bool button = false;
 //
 // This is a state machine.
 // state 0: wait for the start button press.
 // Other states are part of a sequence.
 switch (state)
 { // run mission, initial value
   case 0:
     // read button value
     button = digitalReadFast(PIN_START_BUTTON);
     // A 'start' message from the USB can activate a mission. 
     // This sets the robot.missionStart flag true for five samples.
     if (button or robot.missionStart)
     { // starting a sequence
       //
       desiredValue = 2;
       startTime = millis();
       // update of display takes too long time for fast sampling
       // so, disable during sequence.
       display.useDisplay = false;
       // go to next state
       state = 10;
     }
     break;
   case 10:
     // test if this state is finished
     if (millis() - startTime > 500)
     { // change to next values
       desiredValue = 6;
       state = 20;
     }
     break;
   case 20:
     // test if this state is finished
     if (millis() - startTime > 1000)
     { // change to next values
       desiredValue = 3;
       state = 30;
     }
     break;
   case 30:
     // test if this state is finished
     if (millis() - startTime > 1500)
     { // finished
       desiredValue = 0;
       state = 98;
     }
     break;
   case 98:
     // roll off velocity
     if (millis() - startTime > 2000)
     { // finished
       state = 99;
     }
     break;
   default:
     // back to start
     state = 0;
     desiredValue = 0;
     motor.motorVoltage[0] = 0; // left motor
     motor.motorVoltage[1] = 0;  // right motor
     display.useDisplay = true;
     // print som results
     Serial.print("% Set Sample time ");
     Serial.print(sampleTimeUs);
     Serial.print(" usec. Measured Sample time ");
     Serial.print(encoder.sampleTime_us);
     Serial.print(" usec.");
     Serial.println();
     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

/**

* Global variables to integrate robot position */

float distLeft = 0; float distRight = 0;

These variables are used to send the travelled distance for each wheel.

/**

* 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.print(" ");
     Serial.print(encoder.dEncoder[0]);
     Serial.print(" ");
     Serial.print(encoder.dEncoder[1]);
     Serial.print(" ");
     distLeft += -encoder.motorVelocity[0] * sampleTimeUs * 1e-6 / 9.28 * 0.03;
     distRight += encoder.motorVelocity[1] * sampleTimeUs * 1e-6 / 9.28 * 0.03;
     Serial.print(" ");
     Serial.print(distLeft); // meter
     Serial.print(" ");
     Serial.print(distRight); // meter
     Serial.println();
   }
 }

}

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

This function takes the desiredValue and sets this value as 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).