RobotArm

RobotArm.c

#include "RobotARM.h"

#include "../../tick.h"
#include "../../BrickPi.h"

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

#ifndef NULL
  #define NULL 0
#endif
#define BOOL char
#define FALSE 0
#define TRUE 1

#define DIRECTION_ROTATE_LEFT   0x0001
#define DIRECTION_ROTATE_RIGHT  0x0002
#define DIRECTION_TILT_UP       0x0004
#define DIRECTION_TILT_DOWN     0x0008
#define DIRECTION_GRAB_CLOSE    0x0010
#define DIRECTION_GRAB_OPEN     0x0020
#define DIRECTION_GRAB          0x0040
#define DIRECTION_UNGRAB        0x0080

typedef struct S_ACTION {
    int   m_degree;
    int   m_direction;
} ACTION;


// ----------------------------------------------
// functions to parse user input 
// ----------------------------------------------
char *  ReadLineFromStdIO();
char *  Promt();
BOOL    IsQuit(char *pStr);
BOOL    ToggleVerboseMode(char *pStr);
BOOL    ToggleBlinkLED(char *pStr);
ACTION* Evaluate(char *pStr, int *nCount);
int     _GetDirection(char *pStr);
int     _GetDegree(char *pStr);
void    DoRunActions(ACTION* pActions, int count);


// ----------------------------------------------
// utility functions
// ----------------------------------------------
void SetUARTAtmegaPorts();
void DeviceConfig();
void UpdateTimeout(int nTimeOut);
void StopAllMotors();
void ChangeMotorDegree(int motor, int degree);

// ----------------------------------------------
// entry points of the actual program
// ----------------------------------------------
int RobotMain();

// ----------------------------------------------
// following functions will control the robot
// ----------------------------------------------
void _Rotate(BOOL left, int degree);
void _Tilt(BOOL up, int degree);
void _Grab(BOOL close, int degree);
void __Grab(BOOL close); 
void _ChangeTimeout(BOOL increase);

// ----------------------------------------------
// additional threads
// ----------------------------------------------
pthread_t t_updateBrickPiValues;
pthread_t t_fauUS;
pthread_t t_fauTS;
pthread_t t_fauLS;
pthread_t t_blinkLED;

static void* thread_updateBrickPiValues(void*);
static void* thread_fetchAndUpdateUltrasonicSensorValue(void*);
static void* thread_fetchAndUpdateTouchSensorValue(void*);
static void* thread_fetchAndUpdateLightSensorValue(void*);
static void* thread_blinkLED(void*);

// ----------------------------------------------
// important global variables
// ----------------------------------------------
BOOL    bValidBrickPiState = FALSE;
int     nTimeout = ROBOTARM_DEFAULT_TIMEOUT;
int     nUltrasonicSensorDistance = 0;
int     nLightSensorValue = 0;
int     nTouchSensorValue = 0;
BOOL    bVerbose = FALSE;


char * ReadLineFromStdIO() {
#define BASE_BUFFER_LEN 0xFF
    char * line = malloc(BASE_BUFFER_LEN), * linep = line;
    int lenmax = BASE_BUFFER_LEN, len = lenmax;
    int c;

    if (line == NULL)
        return NULL;

    while(TRUE) {
        c = fgetc(stdin);
        if (c == EOF)
            break;

        if (--len == 0) {
            len = lenmax;
            char * tmp = realloc(linep, lenmax *= 2);

            if (tmp == NULL) {
                free(linep);
                return NULL;
            }
            line = tmp + (line - linep);
            linep = tmp;
        }

        if ((*line++ = c) == '\n')
            break;
    }
    *(line-1) = '\0';
    return linep;
}


char * Promt() {
    printf("> ");
    return ReadLineFromStdIO();
}


BOOL IsQuit(char *pStr) {
    if (strcmp(pStr, "quit") == 0) {
        return TRUE;
    }
    return FALSE;
}


BOOL ToggleVerboseMode(char *pStr) {
    if (strcmp(pStr, "verbose") == 0) {
        bVerbose = !bVerbose;
        return TRUE;
    }
    return FALSE;
}


BOOL ToggleBlinkLED(char *pStr) {
    if (strcmp(pStr, "blink") == 0) {
        pthread_join(t_blinkLED, NULL);
        int nRet;
        nRet = pthread_create(&t_blinkLED, NULL, &thread_blinkLED, NULL);
        if (nRet) {
            printf("failed to setup thread_blinkLED!");
        }
        return TRUE;
    }
    return FALSE;
}

int _GetDirection(char *pStr) {
    if (strcmp(pStr, "left") == 0) {
        return DIRECTION_ROTATE_LEFT;
    } else if (strcmp(pStr, "right") == 0) {
        return DIRECTION_ROTATE_RIGHT;
    } else if (strcmp(pStr, "up") == 0) {
        return DIRECTION_TILT_UP;
    } else if (strcmp(pStr, "down") == 0) {
        return DIRECTION_TILT_DOWN;
    } else if (strcmp(pStr, "close") == 0) {
        return DIRECTION_GRAB_CLOSE;
    } else if (strcmp(pStr, "open") == 0) {
        return DIRECTION_GRAB_OPEN;
    } else if (strcmp(pStr, "grab") == 0) {
        return DIRECTION_GRAB;
    } else if (strcmp(pStr, "ungrab") == 0) {
        return DIRECTION_UNGRAB;
    }
    return -1;
}


int _GetDegree(char *pStr) {
    return atoi(pStr);
}


ACTION* Evaluate(char *pStr, int* nCount) {
    ACTION* pActions = malloc(sizeof(ACTION));
    char * pch;
    pch = (char*)strtok (pStr," ");
    int count;
    for (count = 0; TRUE ;++count) {
        ACTION a;
        if (count != 0) {
            pch = (char*)strtok (NULL, " ");
        }
        if (pch == NULL) {
            break;
        }
        a.m_degree = _GetDegree(pch);
        pch = (char*)strtok (NULL, " ");
        a.m_direction = _GetDirection(pch);
        if (pch == NULL) {
            break;
        }
        if (count != 0) {
            ACTION* tmp = realloc(pActions, (count+1)*sizeof(ACTION));
            pActions = tmp;
        }
        pActions[count] = a;
    }
    *nCount = count;
    return pActions;
}


void DoRunActions(ACTION* pActions, int count) {
    int n;
    for (n = 0; n < count; ++n) {
        ACTION* pAction = &pActions[n];
        switch(pAction->m_direction) {
            case DIRECTION_ROTATE_LEFT:
                _Rotate(TRUE, pAction->m_degree);
                break;
            case DIRECTION_ROTATE_RIGHT:
                _Rotate(FALSE, pAction->m_degree);
                break;
            case DIRECTION_TILT_UP:
                _Tilt(TRUE, pAction->m_degree);
                break;
            case DIRECTION_TILT_DOWN:
                _Tilt(FALSE, pAction->m_degree);
                break;
            case DIRECTION_GRAB_CLOSE:
                _Grab(TRUE, pAction->m_degree);
                break;
            case DIRECTION_GRAB_OPEN:
                _Grab(FALSE, pAction->m_degree);
                break;
            case DIRECTION_GRAB:
                __Grab(TRUE);
                break;
            case DIRECTION_UNGRAB:
                __Grab(FALSE);
                break;
        }
    }
}


// -----------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    printf(" - RobotARM -\n");
    ClearTick();
    int setupResult = BrickPiSetup();
    if (setupResult) {
        printf("ERROR! - failed to initialize BrickPi! (%d)\n", setupResult);
        return setupResult;
    }
    DeviceConfig();
    setupResult = BrickPiSetupSensors();
    if (setupResult) {
        printf("ERROR! - failed to setup sensors! (%d)\n", setupResult);
        return setupResult;
    }
    printf(" robot starting...\n\n");
    return RobotMain();
}


void SetUARTAtmegaPorts() {
    //http://www.dexterindustries.com/forum/?topic=programming-in-c-2/#post-1078
    BrickPi.Address[0] = 1; // UART AtMega 328P Address (needed by BrickPi)
    BrickPi.Address[1] = 2; // UART AtMega 328P Address (needed by BrickPi)
}


void DeviceConfig() {
    wiringPiSetup();
    SetUARTAtmegaPorts();
    BrickPi.MotorEnable[MOTOR_ROTATE]   = TYPE_MOTOR_SPEED;
    BrickPi.MotorEnable[MOTOR_TILT]     = TYPE_MOTOR_SPEED;
    BrickPi.MotorEnable[MOTOR_GRAB]     = TYPE_MOTOR_SPEED;
    BrickPi.Timeout = ROBOTARM_DEFAULT_TIMEOUT;
    BrickPiSetTimeout();

    BrickPi.SensorType[SENSOR_ULTRASONIC]   = TYPE_SENSOR_ULTRASONIC_CONT;
    BrickPi.SensorType[SENSOR_TOUCH]        = TYPE_SENSOR_TOUCH;
    BrickPi.SensorType[SENSOR_LIGHT]        = TYPE_SENSOR_LIGHT_ON;
}


void UpdateTimeout(int nTimeOutToSet) {
    printf("UpdateTimeout: %d -> %d\n", nTimeout, nTimeOutToSet);
    nTimeout = nTimeOutToSet;
    BrickPi.Timeout = nTimeout;
    BrickPiSetTimeout();
}


void StopAllMotors() {
    BrickPi.MotorSpeed[MOTOR_ROTATE] = 0;
    BrickPi.MotorSpeed[MOTOR_TILT] = 0;
    BrickPi.MotorSpeed[MOTOR_GRAB] = 0;
}


static void* thread_updateBrickPiValues(void* pArgs) {
    while (TRUE) {
        bValidBrickPiState = !BrickPiUpdateValues();
        usleep(2*msec);
    }
}


static void* thread_blinkLED(void* pArgs) {
    pinMode(1, OUTPUT);	// PIN 12, GPIO 18
    pinMode(2, OUTPUT); 	// PIN 13, GPIO 27

    int i = 10;
    while( i ){
        if (bVerbose) {
            printf(" blink: %d\n", i);
        }
        digitalWrite(1, 1);
        digitalWrite(2, 0);
        delay(50);
        digitalWrite(1, 0);
        digitalWrite(2, 1);
        delay (50) ;
        i--;
    }
    digitalWrite(1, 0);
    digitalWrite(2, 0);
}

// -------------------------------------------------------
// wait for user input and perform actions
// -------------------------------------------------------
int RobotMain() {
    //start additional threads ...
    int nRet;
    nRet = pthread_create(&t_updateBrickPiValues, NULL, &thread_updateBrickPiValues, NULL);
    if (nRet) {
        printf("failed to setup thread_updateBrickPiValues!");
        return -1;
    }
    nRet = pthread_create(&t_fauUS, NULL, &thread_fetchAndUpdateUltrasonicSensorValue, NULL);
    if (nRet) {
        printf("failed to setup thread_fetchAndUpdateUltrasonicSensorValue!");
        return -1;
    }
    nRet = pthread_create(&t_fauTS, NULL, &thread_fetchAndUpdateTouchSensorValue, NULL);
    if (nRet) {
        printf("failed to setup thread_fetchAndUpdateTouchSensorValue!");
        return -1;
    }
    nRet = pthread_create(&t_fauLS, NULL, &thread_fetchAndUpdateLightSensorValue, NULL);
    if (nRet) {
        printf("failed to setup thread_fetchAndUpdateLightSensorValue!");
        return -1;
    }
    // ...

    printf("\n ... robot started!\n");
    printf(" > HINT: quit the robot by typing 'quit'\n");
    while(TRUE) {
        char *strIn = Promt();
        if (IsQuit(strIn)) {
            break;
        } else if (ToggleVerboseMode(strIn)) {
            continue;
        } else if (ToggleBlinkLED(strIn)) {
            continue;
        } else {
            int nCount;
            ACTION* pActions = Evaluate(strIn, &nCount);
            if (NULL == pActions) {
                printf("ERROR! - please check your input!\n");
            } else {
                DoRunActions(pActions, nCount);
                free(pActions);
            }
        }
        free(strIn);
    }
    printf("\n");
    exit(0);
    return 0;
}


BOOL bStopOnTiltUp = FALSE;

void ChangeMotorDegree(int motor, int degree) {
    int baseDegree = BrickPi.Encoder[motor];
    int targetDegree = baseDegree + degree;
    printf("ChangeMotorDegree[%d]: %d -> %d\n", motor, baseDegree, targetDegree);
    BOOL bReverse = targetDegree < baseDegree ? TRUE : FALSE;
    int nCurrentSpeed = bReverse ? -1 * SPEED_BASE : SPEED_BASE;

    while (TRUE) {
        BrickPi.MotorSpeed[motor] = nCurrentSpeed;
        if (bReverse){
            nCurrentSpeed--;
            if (BrickPi.Encoder[motor] < targetDegree) {
                break;
            }
        }
        else {
            nCurrentSpeed++;
            if (BrickPi.Encoder[motor] > targetDegree) {
                break;
            }
        }
        if (bStopOnTiltUp && BrickPi.Sensor[SENSOR_TOUCH]) {
            if (bVerbose) {
                printf("! tilt movement stopped by touch sensor!\n");
            }
            break;
        }
        usleep(10000);
    }
    BrickPi.MotorSpeed[motor] = 0;
    bStopOnTiltUp = FALSE;
}


// -------------------------------------------------------
// rotate the robot-arm (bool indicates if left or right)
// -------------------------------------------------------
void _Rotate(BOOL left, int degree) {
    if (left) {
        ChangeMotorDegree(MOTOR_ROTATE, (-1) * degree * DEGREE_MULTIPLIER_ROTATE);
    }
    else {
        ChangeMotorDegree(MOTOR_ROTATE, degree * DEGREE_MULTIPLIER_ROTATE);
    }
}


// -------------------------------------------------------
// tilt the robot-arm (bool indicates if up or down)
// -------------------------------------------------------
void _Tilt(BOOL up, int degree) {
    if (up) {
        bStopOnTiltUp = TRUE;
        ChangeMotorDegree(MOTOR_TILT, (-1) * degree * DEGREE_MULTIPLIER_TILT);
    }
    else {
        ChangeMotorDegree(MOTOR_TILT, degree * DEGREE_MULTIPLIER_TILT);
    }
}


// -------------------------------------------------------
// open or close the grab-unit of the robot-arm
// -------------------------------------------------------
void _Grab(BOOL close, int degree) {
    if (close) {
        ChangeMotorDegree(MOTOR_GRAB, (-1) * degree * DEGREE_MULTIPLIER_GRAB);
    }
    else {
        ChangeMotorDegree(MOTOR_GRAB, degree * DEGREE_MULTIPLIER_GRAB);
    }
}


// -------------------------------------------------------
// open or close the grab-unit of the robot-arm (permanent)
// -------------------------------------------------------
void __Grab(BOOL close) {
    if (close) {
      BrickPi.MotorSpeed[MOTOR_GRAB] = SPEED_GRAB_CLOSE;
    } else {
      BrickPi.MotorSpeed[MOTOR_GRAB] = SPEED_GRAB_OPEN;
    }
}


// -------------------------------------------------------
// change the timeout of brick-pi (alters responsitivity)
// -------------------------------------------------------
void _ChangeTimeout(BOOL increase) {
    int nNewTimeout = 0;
    if (increase) {
        nNewTimeout = nTimeout * 2;
    }
    else {
        nNewTimeout = nTimeout / 2;
    }
    UpdateTimeout(nNewTimeout);
}


// #######################################################
// #                sensor - threads                     #
// #######################################################

static void* thread_fetchAndUpdateUltrasonicSensorValue(void* pArgs) {
    int nLastVal = 0;
    nUltrasonicSensorDistance = nLastVal;
    while(TRUE){
      if (bValidBrickPiState){
        nUltrasonicSensorDistance = BrickPi.Sensor[SENSOR_ULTRASONIC];
        //255 & 127 are invalid values!
        if (nUltrasonicSensorDistance!=255 && nUltrasonicSensorDistance!=127) {
            if (nUltrasonicSensorDistance != nLastVal) {
                if (bVerbose) {
                    printf(" [DISTANCE]: %3.1d -> %3.1d \n", nLastVal, nUltrasonicSensorDistance);
                }
                nLastVal = nUltrasonicSensorDistance;
            }
        }
       }
      usleep(msec*10);
    }
}

static void* thread_fetchAndUpdateTouchSensorValue(void* pArgs) {
    int nLastVal = 0;
    nTouchSensorValue = nLastVal;
    while(TRUE){
      if (bValidBrickPiState){
        nTouchSensorValue = BrickPi.Sensor[SENSOR_TOUCH];
        if (nTouchSensorValue != nLastVal) {
            if (bVerbose) {
                printf(" [TOUCH]: %d -> %d \n", nLastVal, nTouchSensorValue);
            }
            nLastVal = nTouchSensorValue;
        }
       }
      usleep(msec*10);
    }
}

static void* thread_fetchAndUpdateLightSensorValue(void* pArgs) {
    int nLastVal = 0;
    nLightSensorValue = nLastVal;
    while(TRUE){
      if (bValidBrickPiState){
        nLightSensorValue = BrickPi.Sensor[SENSOR_LIGHT];
        if (nLightSensorValue != nLastVal) {
            if (bVerbose) {
                printf(" [LIGHT]: %d -> %d \n", nLastVal, nLightSensorValue);
            }
            nLastVal = nLightSensorValue;
        }
       }
      usleep(msec*10);
    }
}

RobotArm.h

#pragma once

#define msec    1000
#define sec     1000000

#define MOTOR_ROTATE        PORT_D
#define MOTOR_TILT          PORT_A
#define MOTOR_GRAB          PORT_C

#define SPEED_BASE                  0x30
#define SPEED_MAX                   0xFF
#define DEGREE_MULTIPLIER_ROTATE    111
#define DEGREE_MULTIPLIER_TILT      20
#define DEGREE_MULTIPLIER_GRAB      1
#define SPEED_GRAB_CLOSE            (-50)
#define SPEED_GRAB_OPEN             (-1*SPEED_GRAB_CLOSE)

#define CODE_LEFT     "left"
#define CODE_RIGHT    "right"
#define CODE_UP       "up"
#define CODE_DOWN     "down"
#define CODE_CLOSE    "close"
#define CODE_OPEN     "open"
#define KEY_TIMEOUT_INC     'i'
#define KEY_TIMEOUT_DEC     'k'

#define SENSOR_ULTRASONIC   PORT_3
#define SENSOR_LIGHT        PORT_4
#define SENSOR_TOUCH        PORT_1

#define ROBOTARM_DEFAULT_TIMEOUT 100