added drive_simple and teleop_timple

Makefile

@@ -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

drive_simple/Makefile

+# 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/drive_simple.c

+/*******************************************************************************
+* 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

lcmtypes/mbot_motor_pwm_t.lcm

+struct mbot_motor_pwm_t
+{
+
+    int64_t utime;
+
+    float left_motor_pwm; // [-1.0..1.0]
+    float right_motor_pwm; // [-1.0..1.0]
+}

mobilebot/Makefile

@@ -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
 
 

python/mbot_teleop_simple.py

+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)