Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
gxiang
Mobilebot
Commits
274618e4
Commit
274618e4
authored
Aug 25, 2020
by
pgaskell
Browse files
added drive_simple and teleop_timple
parent
adb25fde
Changes
6
Hide whitespace changes
Inline
Side-by-side
Makefile
View file @
274618e4
...
...
@@ -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
0 → 100644
View file @
274618e4
# 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
0 → 100644
View file @
274618e4
/*******************************************************************************
* 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
0 → 100644
View file @
274618e4
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
View file @
274618e4
...
...
@@ -20,7 +20,7 @@ INSTALL := install -m 4755
INSTALLDIR
:=
install
-d
-m
755
LINK
:=
ln
-s
-f
LINKDIR
:=
/etc/robot
icscape
LINKDIR
:=
/etc/
lib
robot
control
LINKNAME
:=
link_to_startup_program
...
...
python/mbot_teleop_simple.py
0 → 100644
View file @
274618e4
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
)
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment