Wikibooks

Khepera III Toolbox/The Toolbox/Modules/khepera3

< Khepera III Toolbox | The Toolbox | Modules

The khepera3 module provides access to all sensors and actuators of the robot and it divided into the following parts:

  • General: firmware version and timestamp
  • Motor: left and right motor (accessed individually)
  • Drive: drive system (both motors)
  • Infrared: infrared sensors, in ambient or proximity mode
  • Ultrasound: ultrasound sensors
  • Battery: battery voltage, current and charge level

Even though each part is implemented in a separate file, it is sufficient to include khepera3.h, which includes all other files.

Synopsis

edit 
// Initialize the module
khepera3_init();

// Move the left motor to a specific encoder position
khepera3_motor_stop(&khepera3.motor_left);
khepera3_motor_set_current_position(&khepera3.motor_left, 0);
khepera3_motor_start(&khepera3.motor_left);
khepera3_motor_goto_position(&khepera3.motor_left, 10000);
while (1) {
	khepera3_motor_get_current_speed(&khepera3.motor_left);
	... = khepera3.motor_left.current_speed;
}

// Set the speed of the robot by setting the speed of both wheels
khepera3_drive_start();
khepera3_drive_set_speed(5000, 5000);

// Set the speed in a differential manner (speed, forward coefficient, turn coefficient, turn coefficient maximum)
khepera3_drive_set_speed_differential(5000., 1., 0.2, 1.);

// Read infrared sensor values
khepera3_infrared_proximity();
int value = khepera3.infrared_proximity.sensor[cKhepera3SensorsInfrared_FrontLeft];
for (i = cKhepera3SensorsInfrared_Begin; i < cKhepera3SensorsInfrared_End; i++) {
	weighted_sum += khepera3.infrared_proximity.sensor[i] * weight[i];
}

// Enable and read ultrasound values
khepera3_ultrasound_enable(cKhepera3SensorsUltrasoundBit_Front);
khepera3_ultrasound(cKhepera3SensorsUltrasound_Front);
for (i = 0; i < khepera3.ultrasound.sensor[cKhepera3SensorsUltrasound_Front].echos_count; i++) {
	... = khepera3.ultrasound.sensor[cKhepera3SensorsUltrasound_Front].distance[i];
	... = khepera3.ultrasound.sensor[cKhepera3SensorsUltrasound_Front].amplitude[i];
}

// Read battery voltage
khepera3_battery_voltage();
float voltage_V = (float)khepera3.battery.voltage * 0.0001;

Description

edit

The khepera3 module is build around a statically allocated data structure called khepera3. This structure is hierarchically organized and contains fields for all sensors and actuators. A few fields are set at module initialization (khepera3_init), but most fields are updated by the sensor functions provided by this module.

To get a measurement for any sensor, first call the appropriate sensor function (e.g. khepera3_infrared_proximity). When this function returns, the corresponding fields in the khepera3 data structure will contain the new values. Some sensors also return a timestamp, which is a 32bit-counter incremented at regular time intervals. The interval is not precisely defined, and the counter overflows (i.e. restarts at 0 after having reached 2^32-1). Nevertheless, the timestamp is sometimes useful to find out at what regularity measurements are taken.

For actuator functions (e.g. khepera3_drive_set_speed), on the other hand, the values are directly passed as arguments to the function call, and not through the khepera3 structure. Note that for all drive functions, the left motor value is specified before the right motor.

Error Handling

edit

All functions return either -1 to indicate success, or 0 to indicate failure. Failure usually means that the data could not be tranferred over the I2C bus to the microcontroller in charge of the corresponding sensor or actuator. Such errors may occasionally appear, and most often require to cold reboot (switch off, wait a few seconds, then switch on again) the robot.

Therefore, it is not worth implementing sophisticated error handling procedures - either let the program crash (or not work properly) in such cases, or quit cleanly upon failure.

Multi-Threaded Programs

edit

Since the khepera3 structure is allocated statically, it is not safe to call functions which update the same fields from two different threads. I.e. calling khepera3_infrared_proximity() results in a race condition. The same also applies to the underlying i2cal module, which makes use of statically allocated structures as well. Thread safety interferes with programming simplicity here, which would suffer if these structures were not allocated statically.

API

edit

Since the function and variable names are self-explanatory, we do not provide a huge documentation here, but instead list functions, constants and structures of each module part. For a more exhaustive documentation, refer to the .h files.

The khepera3 Data Structure

edit 
khepera3.
    dspic.
        int i2c_address (const)
        int firmware_version
        int firmware_revision

    motor_left. and motor_right.
        int i2c_address (const)
        int direction (const)
        unsigned int firmware_version
        int current_speed
        int current_position
        int current_torque
        enum eKhepera3MotorStatusFlags status
        enum eKhepera3MotorErrorFlags error

    infrared_ambient. and infrared_proximity.
        int sensor[11]
        int timestamp

    ultrasound.
        sensor[5].
            int echos_count
            int distance[10]
            int amplitude[10]
            int timestamp[10]
        float distance_per_increment (const)

    battery.
        unsigned int voltage
        int current
        int current_average
        unsigned int capacity_remaining_absolute
        unsigned int capacity_remaining_relative
        unsigned int temperature

Motor

edit

Motor functions take one of &(khepera3.motor_left) and &(khepera3.motor_right) as first argument. 

// Motor status flags
enum eKhepera3MotorStatusFlags {
    cKhepera3MotorStatusFlags_Moving = (1 << 0),                    // Movement detected
    cKhepera3MotorStatusFlags_Direction = (1 << 1),                 // Direction (0 = negative, 1 = positive)
    cKhepera3MotorStatusFlags_OnSetpoint = (1 << 2),                // On setpoint
    cKhepera3MotorStatusFlags_NearSetpoint = (1 << 3),              // Near setpoint
    cKhepera3MotorStatusFlags_CommandSaturated = (1 << 4),          // Command saturated
    cKhepera3MotorStatusFlags_AntiResetWindup = (1 << 5),           // Antireset windup active
    cKhepera3MotorStatusFlags_SoftwareCurrentControl = (1 << 6),    // Software current control active
    cKhepera3MotorStatusFlags_SoftStop = (1 << 7),                  // Softstop active
};

// Motor error flags
enum eKhepera3MotorErrorFlags {
    cKhepera3MotorErrorFlags_SampleTimeTooSmall = (1 << 0),         // Sample time too small
    cKhepera3MotorErrorFlags_WatchdogOverflow = (1 << 1),           // Watchdog timer overflow
    cKhepera3MotorErrorFlags_BrownOut = (1 << 2),                   // Brown-out
    cKhepera3MotorErrorFlags_SoftwareStop = (1 << 3),               // Software stopped motor (if softstop enabled)
    cKhepera3MotorErrorFlags_MotorBlocked = (1 << 4),               // Motor blocked (if motor blocking enabled)
    cKhepera3MotorErrorFlags_PositionOutOfRange = (1 << 5),         // Position out of range
    cKhepera3MotorErrorFlags_SpeedOutOfRange = (1 << 6),            // Speed out of range
    cKhepera3MotorErrorFlags_TorqueOutOfRange = (1 << 7),           // Torque out of range
};

// Simple functions
int khepera3_motor_initialize(struct sKhepera3Motor *motor);
int khepera3_motor_stop(struct sKhepera3Motor *motor);
int khepera3_motor_start(struct sKhepera3Motor *motor);
int khepera3_motor_idle(struct sKhepera3Motor *motor);
int khepera3_motor_set_speed(struct sKhepera3Motor *motor, int speed);
int khepera3_motor_set_speed_using_profile(struct sKhepera3Motor *motor, int speed);
int khepera3_motor_goto_position(struct sKhepera3Motor *motor, int position);
int khepera3_motor_goto_position_using_profile(struct sKhepera3Motor *motor, int position);
int khepera3_motor_set_current_position(struct sKhepera3Motor *motor, int position);
int khepera3_motor_firmware_version(struct sKhepera3Motor *motor);
int khepera3_motor_get_status(struct sKhepera3Motor *motor);
int khepera3_motor_get_error(struct sKhepera3Motor *motor);
int khepera3_motor_get_current_speed(struct sKhepera3Motor *motor);
int khepera3_motor_get_current_position(struct sKhepera3Motor *motor);
int khepera3_motor_get_current_torque(struct sKhepera3Motor *motor);

// Low-level functions (a list of registers and control types can be found in khepera3_motor.h)
int khepera3_motor_read_register8_p(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister8 reg, unsigned int *result);
int khepera3_motor_read_register16_p(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister16 reg, unsigned int *result);
int khepera3_motor_read_register32_p(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister32 reg, unsigned int *result);
int khepera3_motor_write_register8(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister8 reg, unsigned int value);
int khepera3_motor_write_register16(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister16 reg, unsigned int value);
int khepera3_motor_write_register32(struct sKhepera3Motor *motor, enum eKhepera3MotorRegister32 reg, unsigned int value);
int khepera3_motor_set_control_type(struct sKhepera3Motor *motor, enum eKhepera3MotorControlType control_type);

Drive

edit

Drive functions act on both motors and therefore use and modify both khepera3.motor_left and khepera3.motor_right in the khepera 3 data structure. 

// Drive functions
void khepera3_drive_stop();
void khepera3_drive_start();
void khepera3_drive_idle();
void khepera3_drive_set_speed(int speed_left, int speed_right);
void khepera3_drive_set_speed_differential(float speed, float forward_coefficient, float differential_coefficient);
void khepera3_drive_set_speed_differential_bounded(float speed, float forward_coefficient, float forward_coefficient_max, float differential_coefficient, float differential_coefficient_max);
void khepera3_drive_set_speed_using_profile(int speed_left, int speed_right);
void khepera3_drive_goto_position(int position_left, int position_right);
void khepera3_drive_goto_position_using_profile(int position_left, int position_right);
void khepera3_drive_set_current_position(int position_left, int position_right);
void khepera3_drive_get_current_speed();
void khepera3_drive_get_current_position();
void khepera3_drive_get_current_torque();

Infrared

edit 
//! Infrared sensors
enum eKhepera3SensorsInfrared {
    cKhepera3SensorsInfrared_BackLeft = 0,
    cKhepera3SensorsInfrared_Left = 1,
    cKhepera3SensorsInfrared_FrontSideLeft = 2,
    cKhepera3SensorsInfrared_FrontLeft = 3,
    cKhepera3SensorsInfrared_FrontRight = 4,
    cKhepera3SensorsInfrared_FrontSideRight = 5,
    cKhepera3SensorsInfrared_Right = 6,
    cKhepera3SensorsInfrared_BackRight = 7,
    cKhepera3SensorsInfrared_Back = 8,
    cKhepera3SensorsInfrared_FloorRight = 9,
    cKhepera3SensorsInfrared_FloorLeft = 10,

    cKhepera3SensorsInfrared_Begin = 0,
    cKhepera3SensorsInfrared_End = 10,
    cKhepera3SensorsInfrared_Count = 11,
    cKhepera3SensorsInfrared_RingBegin = 0,
    cKhepera3SensorsInfrared_RingEnd = 9,
    cKhepera3SensorsInfrared_RingCount = 9,
    cKhepera3SensorsInfrared_FloorBegin = 9,
    cKhepera3SensorsInfrared_FloorEnd = 10,
    cKhepera3SensorsInfrared_FloorCount = 2
};

// Functions
int khepera3_infrared_ambient();
int khepera3_infrared_ambient_p(struct sKhepera3SensorsInfrared *result);
int khepera3_infrared_proximity();
int khepera3_infrared_proximity_p(struct sKhepera3SensorsInfrared *result);

Ultrasound

edit 
// Ultrasound sensors
enum eKhepera3SensorsUltrasound {
    cKhepera3SensorsUltrasound_Left = 0,
    cKhepera3SensorsUltrasound_FrontLeft = 1,
    cKhepera3SensorsUltrasound_Front = 2,
    cKhepera3SensorsUltrasound_FrontRight = 3,
    cKhepera3SensorsUltrasound_Right = 4,

    cKhepera3SensorsUltrasound_Begin = 0,
    cKhepera3SensorsUltrasound_End = 5,
    cKhepera3SensorsUltrasound_Count = 5
};

// Ultrasound sensor bit masks
enum eKhepera3SensorsUltrasoundBit {
    cKhepera3SensorsUltrasoundBit_Left = 1,
    cKhepera3SensorsUltrasoundBit_FrontLeft = 2,
    cKhepera3SensorsUltrasoundBit_Front = 4,
    cKhepera3SensorsUltrasoundBit_FrontRight = 8,
    cKhepera3SensorsUltrasoundBit_Right = 16

    cKhepera3SensorsUltrasoundBit_None = 0,
    cKhepera3SensorsUltrasoundBit_All = 31,
};

// Functions
int khepera3_ultrasound(enum eKhepera3SensorsUltrasound sensor);
int khepera3_ultrasound_p(struct sKhepera3SensorsUltrasoundSensor *result, enum eKhepera3SensorsUltrasound sensor);
int khepera3_ultrasound_enable(enum eKhepera3SensorsUltrasoundBit bitmask);
int khepera3_ultrasound_set_max_echo_number(int max_echo_number);
enum eKhepera3SensorsUltrasound khepera3_ultrasound_getsensorbyname(const char *name, enum eKhepera3SensorsUltrasound defaultvalue);
enum eKhepera3SensorsUltrasoundBit khepera3_ultrasound_getsensorbitbysensor(enum eKhepera3SensorsUltrasound sensor);

Battery

edit 
// Functions
int khepera3_battery_voltage();
int khepera3_battery_current();
int khepera3_battery_current_average();
int khepera3_battery_capacity_remaining_absolute();
int khepera3_battery_capacity_remaining_relative();
int khepera3_battery_temperature();
Retrieved from "https://en.wikibooks.org/w/index.php?title=Khepera_III_Toolbox/The_Toolbox/Modules/khepera3&oldid=3679131"