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
goroyeh
Mobilebot
Commits
a1cbf98c
Commit
a1cbf98c
authored
Aug 26, 2020
by
pgaskell
Browse files
added polarity args
parent
274618e4
Changes
1
Hide whitespace changes
Inline
Side-by-side
drive_simple/drive_simple.c
View file @
a1cbf98c
...
...
@@ -10,6 +10,7 @@
#include
<unistd.h>
#include
<sys/stat.h>
#include
<sys/types.h>
#include
<getopt.h>
#include
<lcm/lcm.h>
#include
"../lcmtypes/mbot_encoder_t.h"
...
...
@@ -31,11 +32,48 @@ 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
mot_l_pol
;
int
mot_r_pol
;
int
enc_l_pol
;
int
enc_r_pol
;
// printed if some invalid argument was given
static
void
__print_usage
(
void
)
{
printf
(
"
\n
"
);
printf
(
"-l {pol} polarity of left motor (-1 or 1)
\n
"
);
printf
(
"-r {pol} polarity of right motor (-1 or 1)
\n
"
);
printf
(
"-le {pol} polarity of left encoder (-1 or 1)
\n
"
);
printf
(
"-re {pol} polarity of right encoder (-1 or 1)
\n
"
);
printf
(
"-h print this help message
\n
"
);
printf
(
"
\n
"
);
}
/*******************************************************************************
* int main()
*
*******************************************************************************/
int
main
(){
int
main
(
int
argc
,
char
*
argv
[]){
//check args
if
(
argc
!=
5
)
{
printf
(
"Usage: test_simple {left motor polarity} {right motor polarity} {left encoder polarity} {right encoder polarity}
\n
"
);
printf
(
"Example: test_simple -1 1 -1 1
\n
"
);
return
0
;
}
mot_l_pol
=
atoi
(
argv
[
1
]);
mot_r_pol
=
atoi
(
argv
[
2
]);
enc_l_pol
=
atoi
(
argv
[
3
]);
enc_r_pol
=
atoi
(
argv
[
4
]);
if
(
(
mot_l_pol
!=
1
)
&
(
mot_l_pol
!=
-
1
)
|
(
mot_r_pol
!=
1
)
&
(
mot_r_pol
!=
-
1
)
|
(
enc_l_pol
!=
1
)
&
(
enc_l_pol
!=
-
1
)
|
(
enc_r_pol
!=
1
)
&
(
enc_r_pol
!=
-
1
)){
printf
(
"Usage: polarities must be -1 or 1"
);
return
0
;
}
// make sure another instance isn't running
if
(
rc_kill_existing_process
(
2
.
0
)
<-
2
)
return
-
1
;
...
...
@@ -78,8 +116,9 @@ int main(){
{
rc_motor_set
(
1
,
0
.
0
);
rc_motor_set
(
2
,
0
.
0
);
printf
(
"timeout...
\
n
"
);
printf
(
"timeout...
\
r
"
);
}
printf
()
// define a timeout (for erroring out) and the delay time
lcm_handle_timeout
(
lcm
,
1
);
rc_nanosleep
(
1E9
/
100
);
//handle at 10Hz
...
...
@@ -99,9 +138,9 @@ int main(){
*******************************************************************************/
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
);
printf
(
"
PWM
CMD: %f | %f "
,
mot_l_pol
*
msg
->
left_motor_pwm
,
mot_r_pol
*
msg
->
right_motor_pwm
);
rc_motor_set
(
1
,
mot_l_pol
*
msg
->
left_motor_pwm
);
rc_motor_set
(
2
,
mot_r_pol
*
msg
->
right_motor_pwm
);
publish_encoder_msg
();
watchdog_timer
=
0
.
0
;
}
...
...
@@ -118,8 +157,8 @@ void publish_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
);
encoder_msg
.
leftticks
=
enc_l_pol
*
rc_encoder_eqep_read
(
1
);
encoder_msg
.
rightticks
=
enc_r_pol
*
rc_encoder_eqep_read
(
2
);
printf
(
" ENC: %lld | %lld "
,
encoder_msg
.
leftticks
,
encoder_msg
.
rightticks
);
mbot_encoder_t_publish
(
lcm
,
MBOT_ENCODER_CHANNEL
,
&
encoder_msg
);
}
\ No newline at end of file
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