604b550315a332bdd7c4d3e76f50a8d8e11447b8
[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / utility / TurnAngleCommand.cpp
1 /*
2  * Copyright (c) 2014, Texas Instruments Incorporated
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * *  Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
33 #include "TurnAngleCommand.h"
34 #include "ZumoMotors.h"
35 #include "IMUManager.h"
37 TurnAngleCommand::TurnAngleCommand(float angle, float timeout)
38 {
39     _angle = angle;
40     spinController = PIDController((float) 6.5, (float) 0.001, (float) 0.0);
41     numSeconds = timeout;
42     initialized = false;
43     targetAngleSet = false;
44     atAngle = false;
45 }
47 void TurnAngleCommand::run(Utilities::MotorInfo &info)
48 {
49     float error;
50     float power;
51     if (!targetAngleSet) {
52         float currHeading = IMUManager::getGyroYaw();
53         targetAngle = Utilities::wrapAngle(currHeading + _angle);
54         targetAngleSet = true;
55         atAngle = false;
56     }
57     else {
58         error = Utilities::wrapAngle(IMUManager::getGyroYaw() - targetAngle);
59         power = spinController.calculate(fabs(error));
60         bool done = Utilities::inRange(fabs(error), (float) -10.0,
61                 (float) 10.0);
62         if (done) {
63             atAngle = true;
64         }
65         if (atAngle) {
66             power = 0;
67         }
68         if (fabs(power) < 10) {
69             power = 0;
70         }
71         power = Utilities::clip(power);
72         /* Note: this is used because powers higher than 250 result
73          * in a buildup of error in the integrated gyro angle
74          */
75         if (power > 250) {
76             power = 250;
77         }
78         if (error > 0) {
79             ZumoMotors::setLeftSpeed(power);
80             ZumoMotors::setRightSpeed(-power);
81         }
82         else {
83             ZumoMotors::setLeftSpeed(-power);
84             ZumoMotors::setRightSpeed(power);
85         }
86         info.error = error;
87         info.leftSpeed = power;
88         info.rightSpeed = -power;
89         info.time = (float) millis();
90     }
91     timedOut = ((millis() - initTime) > ((int) (numSeconds * (float) 1000.0)));
92 }
94 void TurnAngleCommand::initialize()
95 {
96     Utilities::reInitErrorVals();
97     initTime = millis();
98     initialized = true;
99     timedOut = false;
100     atAngle = false;
101     targetAngleSet = false;
104 void TurnAngleCommand::end()
106     ZumoMotors::setLeftSpeed((int) 0);
107     ZumoMotors::setRightSpeed(0);
108     timedOut = true;
109     initialized = false;
112 bool TurnAngleCommand::isFinished()
114     return timedOut || atAngle;
117 bool TurnAngleCommand::hasBeenInitialized()
119     return initialized;
122 bool TurnAngleCommand::isTimedOut()
124     return timedOut;