Skip to content

Instantly share code, notes, and snippets.

@mwallner

mwallner/RobotArm.c

Created Dec 22, 2018
Embed
What would you like to do?
RobotArm
#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);
}
}
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment