Servo and compass test

Page last updated 28 Feb 2010, by Eric Bunyan. 0 replies

To play about with my new mbed I figured I'd test out a servo and compass, getting the servo to keep pointing in the same direction.

As the servo can only turn by 180, I simply get an led to turn on when the servo can't turn to that direction. I also get it to center for a couple of seconds before it starts tracking so it's possible to know where it's actually trying to point.

As always, a compass has issues due to metal and emf in the environment (which means it isn't too happy about the servo that's right by it, or the battery pack which is left on the desk), so although it works ok, the next step is to add the gyro I got a while back and haven't used yet into the mix.

/*
 * Test project to turn an FPS148 servo depending on the direction of a CMPS03 digital compass.
 * If the servo can't turn to a particular direction led 4 will turn on. There are of course
 * plenty of positional errors caused by the general problem with compasses and metal, which
 * are great when you are waving a board around with a noisey servo on it, and a battery pack
 * near by.
 */
#include "mbed.h"
#include "algorithm"
#include 

DigitalOut myled(LED1);

#define SERVO_PERIOD 0.03
#define PULSE_MIN 0.0004
#define PULSE_MAX 0.0022
#define PULSE_CENTER ((PULSE_MIN+PULSE_MAX)/2.0)
#define PULSE_TRAVEL (PULSE_MAX-PULSE_MIN)
#define SERVO1_PIN p21

#define I2C1_PINS p9,p10
#define COMPASS_ADDR 0xC0
#define DEGREE_PULSE_SIZE (PULSE_TRAVEL/1799.5) //for degrees measured from 0-3599, but the servo can only turn 180 degrees

//Serial pc(USBTX, USBRX);
DigitalOut error_led(LED4);

int main() {
    /*init the servo*/
    PwmOut servo1(SERVO1_PIN);
    init_servo(&servo1);
    wait(2);

    /*init the i2c reader*/
    I2C i2c1(I2C1_PINS);
    char compass_read_cmd[2];
    compass_read_cmd[0] = 2;
    char compass_read_result[3];
    short compass_data;

    while(1) {
        i2c1.write(COMPASS_ADDR,compass_read_cmd,1);
        wait(0.05);
        i2c1.read(COMPASS_ADDR,compass_read_result,2);
        //the byte data is in the wrong order, so swap it round before copying to a short
        compass_read_result[2] = compass_read_result[1];
        compass_read_result[1] = compass_read_result[0];
        compass_read_result[0] = compass_read_result[2];
        memcpy(&compass_data,compass_read_result,2);
        float calced = compass_data * DEGREE_PULSE_SIZE;
//        pc.printf("Reading: %d ; Calced: %f ;\n",compass_data,calced);
        set_servo_position(&servo1,calced);
        wait(0.1);
    }
}

/*
 * Sets up the given servo and centers it.
 */
void init_servo(PwmOut *servo) {
    servo->period(SERVO_PERIOD);
    servo->pulsewidth(PULSE_CENTER);
}

/*
 * Sets the position of the servo if it's in the allowed range, otherwise turns on error_led.
 */
void set_servo_position(PwmOut *servo,float position) {
    if ( position >= PULSE_MIN && position <= PULSE_MAX ) {
        servo->pulsewidth(position);
        error_led = 0;
    } else {
        error_led = 1;
    }
}

0 comments

You need to log in to post a comment