Commit 274618e4 authored by pgaskell's avatar pgaskell
Browse files

added drive_simple and teleop_timple

parent adb25fde
......@@ -2,6 +2,7 @@ all:
@make -C lcmtypes --no-print-directory
@make -C mobilebot --no-print-directory
@make -C test_motors --no-print-directory
@make -C drive_simple --no-print-directory
@make -C measure_motor_params --no-print-directory
@make -C python
......@@ -15,6 +16,7 @@ clean:
@make -C lcmtypes -s clean
@make -C mobilebot -s clean
@make -C test_motors -s clean
@make -C drive_simple -s clean
@make -C measure_motor_params -s clean
@make -C java -s clean
@make -C python -s clean
# this target will be compiled
TARGET = ../bin/drive_simple
CC := gcc
LINKER := gcc -o
CFLAGS := -std=c99 -c -Wall -g `pkg-config --cflags lcm`
LFLAGS := -lm -lrt -lpthread -lrobotcontrol -L `pkg-config --libs lcm`
SOURCES := $(wildcard *.c)
SOURCES := $(SOURCES) $(wildcard ../common/*.c)
INCLUDES := $(wildcard *.h)
INCLUDES := $(INCLUDES) $(wildcard ../common/*.h)
OBJECTS := $(SOURCES:$%.c=$%.o)
LCMTYPES := $(shell find ../lcmtypes/ -name '*.lcm')
LCMOBJS := $(LCMTYPES:$%.lcm=$%.o)
prefix := /usr/local
RM := rm -f
INSTALL := install -m 4755
INSTALLDIR := install -d -m 755
LINK := ln -s -f
LINKDIR := /etc/librobotcontrol
LINKNAME := link_to_startup_program
# linking Objects
$(TARGET): $(OBJECTS)
@$(LINKER) $@ $(OBJECTS) $(LCMOBJS) $(LFLAGS)
# compiling command
$(OBJECTS): %.o : %.c $(INCLUDES)
@$(CC) $(CFLAGS) -c $< -o $(@)
@echo "Compiled: "$<
all:
$(TARGET)
debug:
$(MAKE) $(MAKEFILE) DEBUGFLAG="-g -D DEBUG"
@echo " "
@echo "$(TARGET) Make Debug Complete"
@echo " "
install:
@$(MAKE) --no-print-directory
@$(INSTALLDIR) $(DESTDIR)$(prefix)/bin
@$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/bin
@echo "$(TARGET) Install Complete"
clean:
@$(RM) $(OBJECTS)
@$(RM) $(IGNORE)
@$(RM) $(IGNORE:=.o)
@$(RM) $(TARGET)
@echo "$(TARGET) Clean Complete"
uninstall:
@$(RM) $(DESTDIR)$(prefix)/bin/$(TARGET)
@echo "$(TARGET) Uninstall Complete"
runonboot:
@$(MAKE) install --no-print-directory
@$(LINK) $(DESTDIR)$(prefix)/bin/$(TARGET) $(LINKDIR)/$(LINKNAME)
@echo "$(TARGET) Set to Run on Boot"
/*******************************************************************************
* drive_simple.c
*
*
*******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <lcm/lcm.h>
#include "../lcmtypes/mbot_encoder_t.h"
#include "../lcmtypes/mbot_motor_pwm_t.h"
#include <rc/start_stop.h>
#include <rc/encoder_eqep.h>
#include <rc/time.h>
#include <rc/motor.h>
//LCM
lcm_t * lcm;
#define MBOT_ENCODER_CHANNEL "MBOT_ENCODERS"
#define MBOT_MOTOR_PWM_CHANNEL "MBOT_MOTOR_PWM"
//global watchdog_timer to cut off motors if no lcm messages recieved
float watchdog_timer;
//functions
void motor_pwm_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const mbot_motor_pwm_t *msg, void *user);
void publish_encoder_msg();
/*******************************************************************************
* int main()
*
*******************************************************************************/
int main(){
// make sure another instance isn't running
if(rc_kill_existing_process(2.0)<-2) return -1;
// start signal handler so we can exit cleanly
if(rc_enable_signal_handler()==-1){
fprintf(stderr,"ERROR: failed to start signal handler\n");
return -1;
}
if(rc_motor_init()<0){
fprintf(stderr,"ERROR: failed to initialze motors\n");
return -1;
}
lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1");
// make PID file to indicate your project is running
// due to the check made on the call to rc_kill_existing_process() above
// we can be fairly confident there is no PID file already and we can
// make our own safely.
rc_make_pid_file();
//subscribe to pwm commands
mbot_motor_pwm_t_subscribe(lcm,
MBOT_MOTOR_PWM_CHANNEL,
motor_pwm_handler,
NULL);
// done initializing so set state to RUNNING
rc_encoder_eqep_init();
rc_set_state(RUNNING);
watchdog_timer = 0.0;
printf("Running...\n");
while(rc_get_state()==RUNNING){
watchdog_timer += 0.01;
if(watchdog_timer >= 0.25)
{
rc_motor_set(1,0.0);
rc_motor_set(2,0.0);
printf("timeout...\n");
}
// define a timeout (for erroring out) and the delay time
lcm_handle_timeout(lcm, 1);
rc_nanosleep(1E9 / 100); //handle at 10Hz
}
rc_motor_cleanup();
rc_encoder_eqep_cleanup();
lcm_destroy(lcm);
rc_remove_pid_file(); // remove pid file LAST
return 0;
}
/*******************************************************************************
* motor_pwm_handler()
*
* sets motor PWMS from incoming lcm message
*
*******************************************************************************/
void motor_pwm_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const mbot_motor_pwm_t *msg, void *user){
printf("CMD: %f | %f \r",msg->left_motor_pwm,msg->right_motor_pwm);
rc_motor_set(1,msg->left_motor_pwm);
rc_motor_set(2,msg->right_motor_pwm);
publish_encoder_msg();
watchdog_timer = 0.0;
}
/*******************************************************************************
* void publish_encoder_msg()
*
* publishes LCM message of encoder reading
*
*******************************************************************************/
void publish_encoder_msg(){
mbot_encoder_t encoder_msg;
encoder_msg.utime = rc_nanos_since_epoch();
encoder_msg.left_delta = 0;
encoder_msg.right_delta = 0;
encoder_msg.leftticks = rc_encoder_eqep_read(1);
encoder_msg.rightticks = rc_encoder_eqep_read(2);
mbot_encoder_t_publish(lcm, MBOT_ENCODER_CHANNEL, &encoder_msg);
}
\ No newline at end of file
struct mbot_motor_pwm_t
{
int64_t utime;
float left_motor_pwm; // [-1.0..1.0]
float right_motor_pwm; // [-1.0..1.0]
}
......@@ -20,7 +20,7 @@ INSTALL := install -m 4755
INSTALLDIR := install -d -m 755
LINK := ln -s -f
LINKDIR := /etc/roboticscape
LINKDIR := /etc/librobotcontrol
LINKNAME := link_to_startup_program
......
import pygame
from pygame.locals import *
from picamera.array import PiRGBArray
from picamera import PiCamera
import cv2
import time
import numpy as np
import sys
sys.path.append("lcmtypes")
import lcm
from lcmtypes import mbot_motor_pwm_t
FWD_PWM_CMD = 0.5
TURN_PWM_CMD = 0.25
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
pygame.init()
pygame.display.set_caption("MBot TeleOp")
screen = pygame.display.set_mode([640,480])
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))
time.sleep(0.5)
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
image = frame.array
screen.fill([0,0,0])
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
image = image.swapaxes(0,1)
image = cv2.flip(image, -1)
image = pygame.surfarray.make_surface(image)
screen.blit(image, (0,0))
pygame.display.update()
fwd = 0.0
turn = 0.0
for event in pygame.event.get():
if event.type==pygame.QUIT:
pygame.quit()
sys.exit()
cv2.destroyAllWindows()
key_input = pygame.key.get_pressed()
if key_input[pygame.K_LEFT]:
turn += 1.0
if key_input[pygame.K_UP]:
fwd +=1.0
if key_input[pygame.K_RIGHT]:
turn -= 1.0
if key_input[pygame.K_DOWN]:
fwd -= 1.0
command = mbot_motor_pwm_t()
command.left_motor_pwm = -(fwd * FWD_PWM_CMD - turn * TURN_PWM_CMD)
command.right_motor_pwm = fwd * FWD_PWM_CMD + turn * TURN_PWM_CMD
lc.publish("MBOT_MOTOR_PWM",command.encode())
rawCapture.truncate(0)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment