Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Em6
robotique
Commits
0d58475f
Commit
0d58475f
authored
Mar 24, 2016
by
Arnaud CADOT
Browse files
Updated sharp checks for KJ (servos weren't stopping)
parent
00b93aa7
Changes
2
Hide whitespace changes
Inline
Side-by-side
stm32/include/KJ2016Tempo.h
View file @
0d58475f
...
...
@@ -8,7 +8,7 @@
*/
class
KJ2016Tempo
{
static
const
float
KJ_INTERAXIS
=
1
3
2.
f
;
// To update
static
const
float
KJ_INTERAXIS
=
1
4
2.
56
f
;
// To update
static
const
float
KJ_WHEEL_DIAMETER
=
70.
f
;
static
const
float
SERVO_MAX_RPM
=
52.
f
;
...
...
@@ -23,7 +23,12 @@ class KJ2016Tempo
const
unsigned
int
RIGHT_SERVO_ID
;
const
float
SERVO_SPEED_FACTOR
;
unsigned
int
m_leftServoSpeed
;
unsigned
int
m_rightServoSpeed
;
void
stop
();
void
pause
();
void
resume
();
void
turn90
(
bool
toLeft
);
...
...
stm32/src/KJ2016Tempo.cpp
View file @
0d58475f
...
...
@@ -48,19 +48,36 @@ void KJ2016Tempo::run(bool isYellow)
void
KJ2016Tempo
::
stop
()
{
m_leftServoSpeed
=
0
;
m_rightServoSpeed
=
0
;
ServosNumeriques
::
moveAtSpeed
(
0
,
LEFT_SERVO_ID
);
ServosNumeriques
::
moveAtSpeed
(
0
,
RIGHT_SERVO_ID
);
}
void
KJ2016Tempo
::
pause
()
{
ServosNumeriques
::
moveAtSpeed
(
0
,
LEFT_SERVO_ID
);
ServosNumeriques
::
moveAtSpeed
(
0
,
RIGHT_SERVO_ID
);
}
void
KJ2016Tempo
::
resume
()
{
ServosNumeriques
::
moveAtSpeed
(
m_leftServoSpeed
,
LEFT_SERVO_ID
);
ServosNumeriques
::
moveAtSpeed
(
m_rightServoSpeed
,
RIGHT_SERVO_ID
);
}
void
KJ2016Tempo
::
turn90
(
bool
toLeft
)
{
static
const
float
alpha
=
1.08
f
*
(
1000.
f
*
KJ2016Tempo
::
KJ_INTERAXIS
*
3.1415
)
/
(
KJ2016Tempo
::
SERVO_SPEED_FACTOR
*
KJ2016Tempo
::
KJ_WHEEL_DIAMETER
*
2.
f
);
static
const
float
alpha
=
(
1000.
f
*
KJ2016Tempo
::
KJ_INTERAXIS
*
3.1415
)
/
(
KJ2016Tempo
::
SERVO_SPEED_FACTOR
*
KJ2016Tempo
::
KJ_WHEEL_DIAMETER
*
2.
f
);
unsigned
int
angularSpeed
=
1023
;
ServosNumeriques
::
moveAtSpeed
((
toLeft
?
0
:
1023
)
+
angularSpeed
,
RIGHT_SERVO_ID
);
ServosNumeriques
::
moveAtSpeed
((
toLeft
?
0
:
1023
)
+
angularSpeed
,
LEFT_SERVO_ID
);
m_leftServoSpeed
=
(
toLeft
?
0
:
1023
)
+
angularSpeed
;
m_rightServoSpeed
=
(
toLeft
?
0
:
1023
)
+
angularSpeed
;
resume
();
waitForArrival
(
static_cast
<
unsigned
int
>
(
alpha
/
(
float
)
angularSpeed
));
...
...
@@ -73,8 +90,10 @@ void KJ2016Tempo::move(int distance)
unsigned
int
angularSpeed
=
1023
;
ServosNumeriques
::
moveAtSpeed
((
distance
<
0
?
1023
:
0
)
+
angularSpeed
,
LEFT_SERVO_ID
);
ServosNumeriques
::
moveAtSpeed
((
distance
>
0
?
1023
:
0
)
+
angularSpeed
,
RIGHT_SERVO_ID
);
m_leftServoSpeed
=
(
distance
<
0
?
1023
:
0
)
+
angularSpeed
;
m_rightServoSpeed
=
(
distance
>
0
?
1023
:
0
)
+
angularSpeed
;
resume
();
waitForArrival
(
static_cast
<
unsigned
int
>
(
beta
*
(
float
)((
distance
<
0
)
?-
distance
:
distance
)
/
(
float
)
angularSpeed
));
stop
();
...
...
@@ -96,6 +115,9 @@ void KJ2016Tempo::waitForArrival(unsigned int duration)
duration
-=
t
;
while
(
Sensors
::
getSingleton
()
->
sharpDetect
());
// We wait for all sharps to be clear
while
(
Sensors
::
getSingleton
()
->
sharpDetect
())
// We wait for all sharps to be clear
pause
();
resume
();
}
}
Write
Preview
Markdown
is supported
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