servo.c

This example shows you how to control servo motors using the servo motor controller module.

#include <bluesense.h>

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>


int main(int argc, char **argv) {
        char *progname = *argv++; argc--;

        // check argument count
        if(argc < 4) {
          fprintf(stderr, "\n");
          fprintf(stderr, "Please re-run this example with \n");
          fprintf(stderr, "  an identifier, \n");
          fprintf(stderr, "  a bitmask which determines the enabled motors, \n");
          fprintf(stderr, "  a byte value for the minimum position, \n");
          fprintf(stderr, "  a byte value for the maximum position \n");
          fprintf(stderr, "\n");
          fprintf(stderr, "Usage: %s <identifier> <bitmask> <minpos> <maxpos>\n", progname);
          fprintf(stderr, "  eg.: %s 21 255 100 200\n", progname);
          fprintf(stderr, "\n");
          fprintf(stderr, "To find out what devices are connected try the 'list_devices' example.\n");
          fprintf(stderr, "\n");
          return(1);
        }

        // get identifier and side from command line
        identifier_t id = atoi(argv[0]);
        uint8_t mask = (uint8_t)atoi(argv[1]);
        uint8_t mina = (uint8_t)atoi(argv[2]);
        uint8_t maxa = (uint8_t)atoi(argv[3]);

        // allocate memory for an interface table and resigster a known device interfaces
        bsDeviceInterface_registeredDeviceInterfaces_t registered = 
                bsDeviceInterface_allocInterfaceTable();
        
        // allocate memory of a device table and open a master device
        bsDeviceTable_table_t devices = bsDeviceTable_allocDeviceTable();
        bsMaster_device_t master = bsMaster_allocMaster(registered,devices,0);
        //bsMaster_addIPAddress(master, "1558", "admin", "admin");

        printf("Detecting device %d...\n", id);
        fflush(stdout);

        // try to find our device within the next 5 seconds
        bsDeviceTable_deviceInfo_t dev = bsMaster_detectDevice(master, id, 5 * 1000 * 1000);

        // device not found?
        if(!dev) {
                fprintf(stderr, "Device %d not found!\n", id);

                // clean up previously allocated memory
                bsMaster_freeMaster(master);
                bsDeviceTable_freeDeviceTable(devices);
                bsDeviceInterface_freeInterfaceTable(registered);
                return(0);
        }

        // get specific device
        bsServoDevice_specifics_t specDev = bsServoDevice_getDeviceSpecifics(dev);
        // set the angle offset for the motors to 0
        bsServoDevice_requestSetConfiguration(specDev,64);
        // wait one second and call yield every 2 ms
        uint32_t autoYieldParam =0;
        autoYieldParam = bsMaster_autoYield(master,1000000,2000,autoYieldParam);
        // calling yield for one second ensures that the offset will have been changed

        printf("Update motor positions 50 times...\n"); 
        fflush(stdout);

        // initalize the angles of the motors
        uint8_t angles[BLUESENSE_SERVOCONTROLLER_SERVOCOUNT];
        uint8_t servos; 
        uint8_t i;
        
        for(i = 0; i < 100; i++) {
                // invert the angles(toggle)
                printf("Setting servo ");
                for (servos=0; servos<BLUESENSE_SERVOCONTROLLER_SERVOCOUNT; servos++) {
                        if (mask & (1<<servos)) {
                                if ((servos % 2)==(i % 2)) {
                                        angles[servos] = mina;
                                } else {
                                        angles[servos] = maxa;
                                }
                        }
                        printf("(Motor %d,Angle %d)",servos,angles[servos]);
                }
                printf("\n");
                
                // update the servo motor angles, a mask of 255 indicates that all motors
                // should update their angle.
                bsServoDevice_requestSetAngles(specDev, mask, angles);

                fflush(stdout);


                // give BlueSense network some time
                // autoYield for 1 second, each 2 ms call yield
                autoYieldParam = bsMaster_autoYield(master,1000000,2000,autoYieldParam);
        }

        // clean up previously allocated memory
        bsMaster_freeMaster(master);
        bsDeviceTable_freeDeviceTable(devices);
        bsDeviceInterface_freeInterfaceTable(registered);
        return(0);
}       

---