philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix stop function in Servo driver

Browse Source
This commit is contained in:
philsson 2018-08-31 17:49:59 +02:00
parent f2693a4c88
commit cce67f7659
2 changed files with 8 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
main.cpp
View File

@ -124,9 +124,6 @@ int main() {
int wmi = imu.whoami(); int wmi = imu.whoami();
int scale = imu.set_acc_scale(BITS_FS_16G); int scale = imu.set_acc_scale(BITS_FS_16G);
// Stop the sweeping arm and do some other stuff with it
servo.stop();
Thread::wait(1000); Thread::wait(1000);
servo.nod(); servo.nod();
@ -135,9 +132,7 @@ int main() {
servo.sweep(1.0, 1.5, 3); servo.sweep(1.0, 1.5, 3);
servo.stop(); servo.setPosition(-0.5);
servo.setPosition(-1);
while (true) while (true)
{ {

17
src/drivers/servo.cpp
View File

@ -72,8 +72,6 @@ Servo::Servo(PinName servoArm)
void Servo::center() void Servo::center()
{ {
//stop();
setPosition(0.0); setPosition(0.0);
} }
@ -84,10 +82,7 @@ void Servo::resetTimers()
void Servo::setPosition(double position) void Servo::setPosition(double position)
{ {
/* TODO: For now we need to call "stop()" stop();
before setting pos when we know we've ran a thread before
Stop does not work here for some reason */
//stop();
double increment = position*500.0; double increment = position*500.0;
@ -112,6 +107,8 @@ void Servo::invert()
void Servo::sweep(double from, double to, double intervalInSec) void Servo::sweep(double from, double to, double intervalInSec)
{ {
stop();
pServoThread = new Thread; pServoThread = new Thread;
SweepData sweepData; SweepData sweepData;
@ -126,6 +123,8 @@ void Servo::sweep(double from, double to, double intervalInSec)
void Servo::nod() void Servo::nod()
{ {
stop();
NodData nodData; NodData nodData;
nodData.pServo = &m_servo; nodData.pServo = &m_servo;
nodData.latestPosition = m_latestPosition; nodData.latestPosition = m_latestPosition;
@ -139,13 +138,11 @@ void Servo::nod()
void Servo::stop() void Servo::stop()
{ {
if (pServoThread->get_state() != Thread::Deleted) if (pServoThread != NULL && pServoThread->get_state() != Thread::Deleted)
{ {
pServoThread->terminate(); pServoThread->terminate();
}
if (pServoThread != NULL)
{
delete pServoThread; delete pServoThread;
pServoThread = NULL;
} }
} }