motor.c

This example shows you how to control motors using the 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 < 1) {
                fprintf(stderr, "\n");
                fprintf(stderr, "Please re-run this example with \n");
                fprintf(stderr, "  an identifier, \n");
                fprintf(stderr, "\n");
                fprintf(stderr, "Usage: %s <identifier>\n", progname);
                fprintf(stderr, "  eg.: %s 1311\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]);

        // 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);

        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
        bsMotorDevice_specifics_t specDev = bsMotorDevice_getDeviceSpecifics(dev);
        uint32_t autoYieldParam = 0;
        
        autoYieldParam = bsMaster_autoYield(master,1000000,2000,autoYieldParam);
        // calling yield for one second ensures that everything is initialized


        printf("  The 'a'+enter key activates motor 1 (left)\n");
        printf("  The 'b'+enter key activates motor 2 (right)\n");
        printf("  The '-'+enter and '=' key decrease and increase the speed.\n");
        printf( "  The 'q'+enter key quits.\n");
        printf( "\n");

        int8_t motorSpeeds[BLUESENSE_MOTORCONTROLLER_MOTORCOUNT];
        
        int16_t speed = 0;
        float leftright = 0.5f;
        
        float steersteps = 50.0f;
        int8_t speedstep = 4;
        
        while (1) {
                //fd_set fds;
                //FD_ZERO(&fds);
                //FD_SET(0,&fds);
                //struct timeval tv;
                //tv.tv_sec = 0;
                //tv.tv_usec = 0;
                char key;
                //if(select(0+1, &fds, NULL, NULL, &tv) > 0) {
                        key = getchar();
                //} else {
                //      key = 0;
                //}

                                
                if (key=='a') {
                        leftright -=  1.0f/steersteps;
                        if (leftright<0.0f) {
                                leftright = 0.0f;
                        }
                } else if (key=='b') {
                        leftright +=  1.0f/steersteps;
                        if (leftright>1.0f) {
                                leftright = 1.0f;
                        }
                } else if (key=='-') {
                        speed -= speedstep;
                        if (speed<-128) {
                                speed = -128;
                        }
                } else if (key=='=') {
                        speed += speedstep;
                        if (speed>127) {
                                speed = 127;
                        }
                } else if (key=='q') {
                        break;
                }
                
                if (key!=0) {
                        int8_t left = 0;
                        int8_t right = 0;
                        if (leftright>0.5) {
                                left = (int8_t)((2.0f-2.0f*leftright)*speed);
                                right = speed;
                        } else {
                                right = (int8_t)(leftright*2.0f*speed);
                                left = speed;
                        }
                
                        printf("Left %d, Right %d total speed %d\n",left,right,speed);
                        fflush(stdout);
                        motorSpeeds[0] = left;
                        motorSpeeds[1] = right;
                        
                        bsMotorDevice_requestSetSpeeds(specDev,1+2,motorSpeeds);
                }
                
                autoYieldParam = bsMaster_autoYield(master,100*1000,2000,autoYieldParam);
        }
        

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

---