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  */
  32
  33 #include <Energia.h>
  34
  35 #include "Utilities.h"
  36 #include "ZumoMotors.h"
  37 #include "IMUManager.h"
  38
  39 #include "DriveLineCommand.h"
  40
  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 }
  49
  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)));
  78
  79         info.error = error;
  80         info.leftSpeed = lTotal;
  81         info.rightSpeed = rTotal;
  82         info.time = (float) millis();
  83     }
  84 }
  85
  86 void DriveLineCommand::initialize()
  87 {
  88     targetAngle = IMUManager::getGyroYaw();
  89     initTime = millis();
  90     initialized = true;
  91     timedOut = false;
  92 }
  93
  94 void DriveLineCommand::end()
  95 {
  96     ZumoMotors::setLeftSpeed(0);
  97     ZumoMotors::setRightSpeed(0);
  98     timedOut = true;
  99     initialized = false;
 100 }
 101
 102 bool DriveLineCommand::isFinished()
 103 {
 104     return timedOut;
 105 }
 106
 107 bool DriveLineCommand::hasBeenInitialized()
 108 {
 109     return initialized;
 110 }
 111
 112 bool DriveLineCommand::isTimedOut()
 113 {
 114     return timedOut;
 115 }