Fejemis Teensy

Back to fejemis

Drive Teensy

Block diagram with all interfaces and interface protocol should be here

USB Interface

The following list shows the result of sending the fejemis_drive a 'help' command (taken from the GUI when connected to the USB directly):

    help    This help text.
------ State settings -------
   setid N         Set device index to N (id=91, part of hbt).
   setname string  Set device name to string (< 32 chars) should be 'front'.
   brush 0/1       Turn on/off for power to brush
   hbti and 'sub hbt N'    Get time and state (heartbear)
   idi and 'sub id N'      Get device ID (name and number)
----- LED control -----
   led N R G B     Set led number N (0..11) to this RGB value (0..255).
   ledb R G B      Set band color RGB value (0..255).
   leds    Set to default values
   ledon V         Turn LED strip op V=1 or off V=0
---- EE (configuration flash) --------
   eew     save configuration to EE-Prom (flash)
   eer     Read configuration from EE-Prom
------ Motor help -------
   motv m1 time    Set motor voltage -12.0..12.0 for this time (in seconds)
   pwmf F          Set PWM frequency (is=15000)
   moti and 'sub mot N'    Get motor voltage 'mot m1(V) 0 0 0 0 PWM-frq(Hz)'
------ Encoder help -------
   enc0    Reset position and encoder to 0
------ Sensor -------
   adci and 'sub adc N'    Get raw ADC values (12V, current, IR1, IR2, motorV)
   inti and 'sub int N'    Get number of interrupts for ADC1 and ADC2
   bati and 'sub bat N'    Get battery voltage and current (volt [V], amp [A], lipo cells [N]
   timei and 'sub time N'  Get timing info (cycle time[us], sense [us], +control[us], +logetc [us], adc cycle [us], adc cycles/control)
   curi and 'sub cur N'    Get current [A], ad4, ad5, adFactor, offset0, offset1
------ IR distance -------
   irc A1 B1 A2 B2         Set calibration values A=13cm, B=50cm (sensor 1 and 2)
   iri and 'sub ir N'      Get IR and calibration data 'irc dist1 dist2 ... (meter)
------ Steering servo -------
   svp P T Set servo position (P=0..1000, time T 0..254).
   svv V T Set servo velocity (V=0..1000, acc-time T= 0..254).
   svat T  Set acceleration time (T=0 rectangle, M>0 is M*11.2ms) - no effect in vel mode?.
   svar R  Set acceleration ratio (R=0 fast, R>slower) - no effect in vel mode?.
   svjt T  Set servo jog time (T=0..255 x 11.2ms)
   svb B   Set servo communication Baud rate (default=115200)
   svscan Id       Scan for servos on serial bus from Id = 0..254.
   svpi and 'sub svp N'    Get current servo position
   svjti and 'sub svjt N'  Get jog time, accRatio, accMax time ('svjt jog ratio max')
------ Front wheel position -------
   pds     Print debug string from AS driver.
   read V  Read angle or not (debug).
   fwoffset A I    Set front wheel offset (deg), I=1 invert angle.
   fwli and 'sub fwl N'    Get current front wheel angle 'fwl ang(deg) err disg'
   fwoi and 'sub fwo N'    Get front wheel offset 'fwo ofset reverse'
------ Steering control -------
   cssteer E ...   Set front angle controller (see ucontrolbase.cpp for params).
   confs B S       Set wheel base and steer base (in meters)
   cvsteeri and 'sub cvsteer N'    Get current control values
   cpsteeri and 'sub cpsteer N'    Get control parameters
   confsi and 'sub confs N'        Get robot steer config 'confs steer-base wheel-bae' (meters)

The '(Ru) :' is added by the GUI - stands for: received from USB.

Turning control

The turning in the drive unit receives two messages:

• Linear velocity desired (v in m/sec) and turnrate w in radians/sec (RC message)
• Angle of front-wheel (in degrees) (fwl message)

The angle of the front wheel is used to calculate a turning radius d.

d = f/tan(theta) + B/2

where f is the distance to the steering wheel, theta is the angle of the front wheel and B is the drive base, the distance between the driving wheels.

The turn radius d is from the turning centre to the other wheel (the fastest wheel). This wheel should have the desired velocity v2.

v2 = sqrt(v^2 + w^2)

with the sign from the linear velocity v. w is assumed to be in radians, and with the scale of the robot (B = 0.4m), it seems OK.

The inner wheel should then have a velocity v1 dependent on the turn radius d and the velocity of the other wheel v2

v1 = v2 - v2 * B / d

The sign of theta determines which wheel should have the higher velocity.

Front Teensy

USB interface

The allowed commands to the front Teensy are:

(@todo)

Turning control

The front wheel receives the movement command (RC) with

• desired linear velocity v
• desired turn rate w

The associated turn angle theta of the front wheel can then be calculated as

theta = atan2(f * w, v)

where f is the distance to the front wheel, w is the desired turn rate, B is the wheelbase between the driving wheels and v is the desired linear velocity.

The angle is presented to a servo control loop and the actual steering angle is measured and send to the drive Teensy (through the bridge).

The theta angle is allowed in the range of -90 deg to +90 deg.

Teensy Software structure

Figure: The Teensy software is structured with a main loop and a number of units. After reset all units are initialized in a setup function, after that the main loop is entered. The main loop services the USB and send commands to the units for decoding. At regular intervals, the sample clock tick, all units are called to execute any sample time function. Most units are interfaces to external devices such as IMU, motor drive or distance sensor. There is further support units for e.g. control.

Setup - loop overview

The setup and loop structure follows the Arduino sketch format. The file is a C++ file as the compilation is using a Makefile rather then the Arduino IDE.

The main file (main.cpp) code has this structure (shortened for clarity)

void setup()   // INITIALIZATION
{
 usb.setup();
 led.setup();
 imu.setup();
 enc.setup();
 sensor.setup();
 motor.setup();
 state.setup();
}
void loop(void)
{
 usb.send("# Starting main loop\n");
 while ( true ) 
 { // main loop
     usb.tick();     // incoming command service
   if ( startNewCycle ) // start of new control cycle
   { // mostly every 1ms
     imu.tick();     // for heading estimate
     sensor.tick();  // AD converter, e.g. battery maintenance
     irdist.tick();  // distance sensor
     enc.tick();     // wheel encoder and odometry
     state.tick();   // e.g. emergency stop
     control.tick(); // feedback control
     motor.tick();   // motor control
   }
 }
}

Unit structure

Figure: Each unit is coded in a separate file and has about the same structure. The unit is a class that holds the data from or to the device. The device is configured when the setup is called. The data from the device, or calculated from the data, is made available for subscription using a subscription class called USubss. The device is accessed using a standard Arduino-type library, for e.g. a I2C or an A/D interface. The unit may have configuration that need to be saved, e.g. calibration data or device mode that can be set from the interface, but needs to be saved.

Command decode

Commands from the USB connection is line oriented and consist of one line of text starting with a keyword. The line ends with a "new-line" (\n) or a combination if "carriage-return" (\r) and "new-line".

Request of data can be just the keyword, e.g.:

help   (for on-line help - list of available commands)
gyroi  (for one-time information about gyro measurement)
bati   (for one-time info about battery voltage)

or a keyword with parameters

ctrli vel  (to get all the PID control parameters for velocity control)

Issue commands or send data, e.g.:

motv m1 m2  (to set motor 1 (left) and motor 2 (right) to a specified voltage.
rc e v t    (to do remote drive control with velocity "v" and turn-rate "t") "e" should be 2 for non-manual control.

Subscription

From the USB most data can be subscribed with the a line starting with the keyword "sub", e.g.:

sub gyro 10  (for subscription to gyro data every 10ms)
sub bat 200  (for subscription to battery status every 200ms)

The available subscriptions are generated in the "setup" part of the unit and holds also the on-line help text.

E.g. the wheel encoder unit has this subscription setup code (in enc.cpp):

void setup()
{
 ...
 addPublistItem("enc", "Get encoder value 'enc encoder interrupts' (integer)");
 addPublistItem("pose", "Get current pose 'pose t x y h' (sec,m,m,rad)");
 addPublistItem("vel", "Get velocity 'left right' (m/s)");
 addPublistItem("conf", "Get robot configuration (radius, radius, gear, pulsPerRev, wheelbase, sample-time)");
}

At tick-time subscriptions are serviced, and if it is time, the "sendData(item)" is called, e.g. for the wheel encoder unit:

void UEncoder::sendData(int item)
{
 if (item == 0)
   sendEncStatus();
 else if (item == 1)
   sendPose();
 else if (item == 2)
   sendVelocity();
 else if (item == 3)
   sendRobotConfig();
}

Configuration save

The Teensy has 4096 bytes of configuration flash memory, this is used to store settings and calibration values.

The use of this memory is controlled by the "config save" unit in the file "eeconfig.cpp".

The unit is controlled by two commands "eew" (save) and "eer" (load). On reset the configuration is loaded.

If the ID number is set to "0", all configurations are reset (to factory settings). To save any configuration, set ID number with the command "setid N" where N is a number > 0.

Configuration is saved as a stack, and each unit must therefore save and load exactly the same amount of data. Add load and save to the list in "eeconfig.cpp" when new units are added that need configuration saved.

The load and save are like e.g.:

void UImu::eePromSave()
{
 eeConfig.push32(offsetGyro[0]);
 eeConfig.push32(offsetGyro[1]);
 eeConfig.push32(offsetGyro[2]);
 // accelerometer
 for (int i = 0; i < 3; i++)
   eeConfig.pushFloat(accOffset[i]);
}

void UImu::eePromLoad()
{
 offsetGyro[0] = eeConfig.read32();
 offsetGyro[1] = eeConfig.read32();
 offsetGyro[2] = eeConfig.read32();
 gyroOffsetDone = true;
 //
 for (int i = 0; i < 3; i++)
   accOffset[i] = eeConfig.readFloat();
}

Update tick

The timing is based on a 10 us timer and the following is defined (in "main.cpp")

// control period is time between control calls
// and is in units of 10us, ie 125 is 1.25ms or 800Hz
#define CONTROL_PERIOD_10us 100
// sample time in seconds
#define SAMPLETIME  (0.00001 * CONTROL_PERIOD_10us)

The SAMPLETIME is a floating point number in seconds. The example above is for 1ms sample time.

All units tick() function is called at this interval. For new units the call must be added in the main code, like for the motor unit:

void UMotor::tick()
{ // 
  implementMotorVoltage();
  // subscription
  subscribeTick();
}

It there is data subscription associated with this unit, then the subscribeTick() must be called to service the subscriptions.