0b665bda02d84eb604d0dd473a95156261beee8b
[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / utility / DriveLineCommand.cpp
1 /*
2  * Copyright (c) 2015, 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 <Energia.h>
35 #include "Utilities.h"
36 #include "ZumoMotors.h"
37 #include "IMUManager.h"
39 #include "DriveLineCommand.h"
41 DriveLineCommand::DriveLineCommand(float speed, float timeout)
42 {
43     _speed = speed;
44     _forward = _speed > 0;
45     steerController = PIDController(0.03, 0.0, 0.0);
46     numSeconds = timeout;
47     initialized = false;
48 }
50 void DriveLineCommand::run(Utilities::MotorInfo &info)
51 {
52     if (!timedOut) {
53         float error = Utilities::wrapAngle(
54                 IMUManager::getGyroYaw() - targetAngle);
55         float power = steerController.calculate(error);
56         power = Utilities::saturate(power, _speed - (float) 1.0,
57                 (float) 1.0 - _speed);
58         float lTotal = Utilities::clip((_speed + power) * 400);
59         float rTotal = Utilities::clip((_speed - power) * 400);
60         if (!_forward) {
61             float temp = lTotal;
62             lTotal = -rTotal;
63             rTotal = -temp;
64         }
65         if (!timedOut) {
66             Serial.print("lTotal: ");
67             Serial.print(lTotal);
68             Serial.print(" rTotal: ");
69             Serial.println(rTotal);
70             ZumoMotors::setLeftSpeed(lTotal);
71             ZumoMotors::setRightSpeed(rTotal);
72         }
73         else {
74             ZumoMotors::setLeftSpeed(0);
75             ZumoMotors::setRightSpeed(0);
76         }
77         timedOut = ((millis() - initTime) > ((int) (numSeconds * 1000.0)));
79         info.error = error;
80         info.leftSpeed = lTotal;
81         info.rightSpeed = rTotal;
82         info.time = (float) millis();
83     }
84 }
86 void DriveLineCommand::initialize()
87 {
88     targetAngle = IMUManager::getGyroYaw();
89     initTime = millis();
90     initialized = true;
91     timedOut = false;
92     Utilities::reInitErrorVals();
93 }
95 void DriveLineCommand::end()
96 {
97     ZumoMotors::setLeftSpeed(0);
98     ZumoMotors::setRightSpeed(0);
99     timedOut = true;
100     initialized = false;
103 bool DriveLineCommand::isFinished()
105     return timedOut;
108 bool DriveLineCommand::hasBeenInitialized()
110     return initialized;
113 bool DriveLineCommand::isTimedOut()
115     return timedOut;