Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

motors.cpp@0:f3bf6c7e2283, 2012-04-20 (annotated)

Committer:
narshu
Date:
Fri Apr 20 20:39:35 2012 +0000
Revision:
0:f3bf6c7e2283
inverted sonar echo input at pin14 and pin15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:f3bf6c7e2283 1 /**********************************************************
narshu 0:f3bf6c7e2283 2 * Motors.cpp
narshu 0:f3bf6c7e2283 3 *
narshu 0:f3bf6c7e2283 4 * This is a motor control library for the UK1122 L298N based motor controller.
narshu 0:f3bf6c7e2283 5 * Includes PID controller to control motors speeds
narshu 0:f3bf6c7e2283 6 *
narshu 0:f3bf6c7e2283 7 * Author: Crispian Poon
narshu 0:f3bf6c7e2283 8 * Email: pooncg@gmail.com
narshu 0:f3bf6c7e2283 9 * Purpose: Eurobot 2012 - ICRS Imperial College London
narshu 0:f3bf6c7e2283 10 * Date: 4th April 2012
narshu 0:f3bf6c7e2283 11 * Version: v0.12
narshu 0:f3bf6c7e2283 12 **********************************************************/
narshu 0:f3bf6c7e2283 13
narshu 0:f3bf6c7e2283 14 #include "mbed.h"
narshu 0:f3bf6c7e2283 15 #include "motors.h"
narshu 0:f3bf6c7e2283 16 #include "QEI.h" //quadrature encoder library
narshu 0:f3bf6c7e2283 17 #include "globals.h"
narshu 0:f3bf6c7e2283 18 #include "TSH.h"
narshu 0:f3bf6c7e2283 19
narshu 0:f3bf6c7e2283 20 //*************************************************************************************
narshu 0:f3bf6c7e2283 21 // Constructor
narshu 0:f3bf6c7e2283 22 //*************************************************************************************
narshu 0:f3bf6c7e2283 23 Motors::Motors():
narshu 0:f3bf6c7e2283 24 current1(p15), current2(p16),
narshu 0:f3bf6c7e2283 25 Encoder1 (p30, p29, NC, 1856 ,QEI::X4_ENCODING), //connects to motor1 quadracture encoders
narshu 0:f3bf6c7e2283 26 Encoder2 (p27, p28, NC, 1856 ,QEI::X4_ENCODING), //connects to motor2 quadracture encoders
narshu 0:f3bf6c7e2283 27 Motor1A(p17), Motor1B(p18), Motor2A(p19), Motor2B(p13), //connects to direction pins
narshu 0:f3bf6c7e2283 28 Motor1Enable(p25), Motor2Enable(p26), //Connects to control board enable pins to control motors speeds. PWM pins. Remember enable must be set before the direction pins changed!!
narshu 0:f3bf6c7e2283 29 PIDControllerMotor1(3.5, 0.5, 0, 0.010), PIDControllerMotor2(3.5, 0.5, 0, 0.010) {
narshu 0:f3bf6c7e2283 30
narshu 0:f3bf6c7e2283 31 //Initialise PID controllers
narshu 0:f3bf6c7e2283 32 PIDControllerMotor1.setMode(MANUAL_MODE);
narshu 0:f3bf6c7e2283 33 PIDControllerMotor2.setMode(MANUAL_MODE);
narshu 0:f3bf6c7e2283 34 PIDControllerMotor1.setBias(-16);
narshu 0:f3bf6c7e2283 35 PIDControllerMotor2.setBias(-16);
narshu 0:f3bf6c7e2283 36 PIDControllerMotor1.setOutputLimits(-127, 127);
narshu 0:f3bf6c7e2283 37 PIDControllerMotor2.setOutputLimits(-127, 127);
narshu 0:f3bf6c7e2283 38 PIDControllerMotor1.setInputLimits(-102, 102);
narshu 0:f3bf6c7e2283 39 PIDControllerMotor2.setInputLimits(-102, 102);
narshu 0:f3bf6c7e2283 40 _lastEncoder1 = 0;
narshu 0:f3bf6c7e2283 41 _lastEncoder2 = 0;
narshu 0:f3bf6c7e2283 42
narshu 0:f3bf6c7e2283 43 };
narshu 0:f3bf6c7e2283 44
narshu 0:f3bf6c7e2283 45
narshu 0:f3bf6c7e2283 46 //*************************************************************************************
narshu 0:f3bf6c7e2283 47 // Public functions
narshu 0:f3bf6c7e2283 48 //*************************************************************************************
narshu 0:f3bf6c7e2283 49
narshu 0:f3bf6c7e2283 50 //*********************************************
narshu 0:f3bf6c7e2283 51 //
narshu 0:f3bf6c7e2283 52 // @Description speed regulator task using PID. Run every 10ms.
narshu 0:f3bf6c7e2283 53 //
narshu 0:f3bf6c7e2283 54 //*********************************************
narshu 0:f3bf6c7e2283 55 void Motors::speedRegulatorTask() {
narshu 0:f3bf6c7e2283 56
narshu 0:f3bf6c7e2283 57 int latestMotor1Speed = 0;
narshu 0:f3bf6c7e2283 58 int latestMotor2Speed = 0;
narshu 0:f3bf6c7e2283 59 int computedSpeed1 = 0;
narshu 0:f3bf6c7e2283 60 int computedSpeed2 = 0;
narshu 0:f3bf6c7e2283 61
narshu 0:f3bf6c7e2283 62 //acceleration control
narshu 0:f3bf6c7e2283 63 if (accelerationRegister == 1) {
narshu 0:f3bf6c7e2283 64 if (_accelerationSpeed1 != 0) {
narshu 0:f3bf6c7e2283 65
narshu 0:f3bf6c7e2283 66 if (abs(_motorSpeed1) < abs(_accelerationSpeed1)) {
narshu 0:f3bf6c7e2283 67 _motorSpeed1 += getSignOfInt(_accelerationSpeed1);
narshu 0:f3bf6c7e2283 68 } else if (abs(_motorSpeed1) > abs(_accelerationSpeed1)) {
narshu 0:f3bf6c7e2283 69 _motorSpeed1 = _accelerationSpeed1;
narshu 0:f3bf6c7e2283 70 }
narshu 0:f3bf6c7e2283 71 }
narshu 0:f3bf6c7e2283 72 if (_accelerationSpeed2 != 0) {
narshu 0:f3bf6c7e2283 73 if (abs(_motorSpeed2) < abs(_accelerationSpeed2)) {
narshu 0:f3bf6c7e2283 74 _motorSpeed2 += getSignOfInt(_accelerationSpeed2);
narshu 0:f3bf6c7e2283 75 } else if (abs(_motorSpeed2) > abs(_accelerationSpeed2)) {
narshu 0:f3bf6c7e2283 76 _motorSpeed2 = _accelerationSpeed2;
narshu 0:f3bf6c7e2283 77 }
narshu 0:f3bf6c7e2283 78 }
narshu 0:f3bf6c7e2283 79 }
narshu 0:f3bf6c7e2283 80
narshu 0:f3bf6c7e2283 81
narshu 0:f3bf6c7e2283 82 //MOTOR 1 PID
narshu 0:f3bf6c7e2283 83 latestMotor1Speed = getEncoder1() - _lastEncoder1; //motor1 encoder change
narshu 0:f3bf6c7e2283 84 //PID setpoints for 50ms interval.
narshu 0:f3bf6c7e2283 85 if (_motorSpeed1 == 0) {
narshu 0:f3bf6c7e2283 86 PIDControllerMotor1.setSetPoint(0);
narshu 0:f3bf6c7e2283 87 } else {
narshu 0:f3bf6c7e2283 88 PIDControllerMotor1.setSetPoint((int)(102*((float)_motorSpeed1/127)));
narshu 0:f3bf6c7e2283 89 }
narshu 0:f3bf6c7e2283 90 //Process value
narshu 0:f3bf6c7e2283 91 PIDControllerMotor1.setProcessValue(latestMotor1Speed);
narshu 0:f3bf6c7e2283 92 //PID Compute
narshu 0:f3bf6c7e2283 93 computedSpeed1 = (int)PIDControllerMotor1.compute();
narshu 0:f3bf6c7e2283 94
narshu 0:f3bf6c7e2283 95
narshu 0:f3bf6c7e2283 96
narshu 0:f3bf6c7e2283 97 //MOTOR 2 PID
narshu 0:f3bf6c7e2283 98 latestMotor2Speed = getEncoder2() - _lastEncoder2; //motor2 encoder change
narshu 0:f3bf6c7e2283 99 //PID setpoints for 50ms interval.
narshu 0:f3bf6c7e2283 100 if (_motorSpeed2 == 0) {
narshu 0:f3bf6c7e2283 101 PIDControllerMotor2.setSetPoint(0);
narshu 0:f3bf6c7e2283 102 } else {
narshu 0:f3bf6c7e2283 103 PIDControllerMotor2.setSetPoint((int)(102*((float)_motorSpeed2/127)));
narshu 0:f3bf6c7e2283 104 }
narshu 0:f3bf6c7e2283 105 //Process value
narshu 0:f3bf6c7e2283 106 PIDControllerMotor2.setProcessValue(latestMotor2Speed);
narshu 0:f3bf6c7e2283 107 //PID Compute
narshu 0:f3bf6c7e2283 108 computedSpeed2 = (int)PIDControllerMotor2.compute();
narshu 0:f3bf6c7e2283 109
narshu 0:f3bf6c7e2283 110
narshu 0:f3bf6c7e2283 111
narshu 0:f3bf6c7e2283 112 //debug variables
narshu 0:f3bf6c7e2283 113 _debug1 = latestMotor1Speed;
narshu 0:f3bf6c7e2283 114 _debug2 = computedSpeed1;
narshu 0:f3bf6c7e2283 115
narshu 0:f3bf6c7e2283 116
narshu 0:f3bf6c7e2283 117
narshu 0:f3bf6c7e2283 118 //Set motors speed
narshu 0:f3bf6c7e2283 119 _setSpeed(computedSpeed1, computedSpeed2);
narshu 0:f3bf6c7e2283 120
narshu 0:f3bf6c7e2283 121 }
narshu 0:f3bf6c7e2283 122
narshu 0:f3bf6c7e2283 123 //*********************************************
narshu 0:f3bf6c7e2283 124 //
narshu 0:f3bf6c7e2283 125 // @Description External set speed function for both motors
narshu 0:f3bf6c7e2283 126 // @Parameter [speed] ranges from -127 (revse motor) to 127 (forward motor)
narshu 0:f3bf6c7e2283 127 //
narshu 0:f3bf6c7e2283 128 //*********************************************
narshu 0:f3bf6c7e2283 129 void Motors::setSpeed(int speed) {
narshu 0:f3bf6c7e2283 130 setSpeed(speed, speed);
narshu 0:f3bf6c7e2283 131 }
narshu 0:f3bf6c7e2283 132
narshu 0:f3bf6c7e2283 133 //*********************************************
narshu 0:f3bf6c7e2283 134 //
narshu 0:f3bf6c7e2283 135 // @Description External set speed function. Relies on the speedRegulatorTask to change speed.
narshu 0:f3bf6c7e2283 136 // @Parameters [speed1] min -127 (reverse motor), max 127 (forward motor)
narshu 0:f3bf6c7e2283 137 // @Parameters [speed2] min -127 (reverse motor), max 127 (forward motor)
narshu 0:f3bf6c7e2283 138 //
narshu 0:f3bf6c7e2283 139 //*********************************************
narshu 0:f3bf6c7e2283 140 void Motors::setSpeed(int speed1, int speed2) {
narshu 0:f3bf6c7e2283 141 //set global motor values
narshu 0:f3bf6c7e2283 142 _motorSpeed1 = speed1;
narshu 0:f3bf6c7e2283 143 _motorSpeed2 = speed2;
narshu 0:f3bf6c7e2283 144 _lastEncoder1 = getEncoder1();
narshu 0:f3bf6c7e2283 145 _lastEncoder2 = getEncoder2();
narshu 0:f3bf6c7e2283 146
narshu 0:f3bf6c7e2283 147 //acceleration control
narshu 0:f3bf6c7e2283 148 if (accelerationRegister == 1) {
narshu 0:f3bf6c7e2283 149 //target accelerated speed
narshu 0:f3bf6c7e2283 150 _accelerationSpeed1 = speed1;
narshu 0:f3bf6c7e2283 151 _accelerationSpeed2 = speed2;
narshu 0:f3bf6c7e2283 152
narshu 0:f3bf6c7e2283 153 //current speed
narshu 0:f3bf6c7e2283 154 _motorSpeed1 = 0;
narshu 0:f3bf6c7e2283 155 _motorSpeed2 = 0;
narshu 0:f3bf6c7e2283 156 }
narshu 0:f3bf6c7e2283 157 }
narshu 0:f3bf6c7e2283 158
narshu 0:f3bf6c7e2283 159 //*********************************************
narshu 0:f3bf6c7e2283 160 //
narshu 0:f3bf6c7e2283 161 // @Description stops motors
narshu 0:f3bf6c7e2283 162 //
narshu 0:f3bf6c7e2283 163 //*********************************************
narshu 0:f3bf6c7e2283 164 void Motors::stop()
narshu 0:f3bf6c7e2283 165 {
narshu 0:f3bf6c7e2283 166 setSpeed(0);
narshu 0:f3bf6c7e2283 167 }
narshu 0:f3bf6c7e2283 168
narshu 0:f3bf6c7e2283 169 //*********************************************
narshu 0:f3bf6c7e2283 170 //
narshu 0:f3bf6c7e2283 171 // @Description resets motor1 and motor encoders
narshu 0:f3bf6c7e2283 172 //
narshu 0:f3bf6c7e2283 173 //*********************************************
narshu 0:f3bf6c7e2283 174 void Motors::resetEncoders() {
narshu 0:f3bf6c7e2283 175 Encoder1.reset();
narshu 0:f3bf6c7e2283 176 Encoder2.reset();
narshu 0:f3bf6c7e2283 177 }
narshu 0:f3bf6c7e2283 178
narshu 0:f3bf6c7e2283 179 //*********************************************
narshu 0:f3bf6c7e2283 180 //
narshu 0:f3bf6c7e2283 181 // @Description gets motor1 encoder
narshu 0:f3bf6c7e2283 182 // @returns motor1 encoder counts
narshu 0:f3bf6c7e2283 183 //
narshu 0:f3bf6c7e2283 184 //*********************************************
narshu 0:f3bf6c7e2283 185 int Motors::getEncoder1() {
narshu 0:f3bf6c7e2283 186 return Encoder1.getPulses();
narshu 0:f3bf6c7e2283 187 }
narshu 0:f3bf6c7e2283 188
narshu 0:f3bf6c7e2283 189 //*********************************************
narshu 0:f3bf6c7e2283 190 //
narshu 0:f3bf6c7e2283 191 // @Description gets motor2 encoder
narshu 0:f3bf6c7e2283 192 // @returns motor2 encoder counts
narshu 0:f3bf6c7e2283 193 //
narshu 0:f3bf6c7e2283 194 //*********************************************
narshu 0:f3bf6c7e2283 195 int Motors::getEncoder2() {
narshu 0:f3bf6c7e2283 196 return Encoder2.getPulses();
narshu 0:f3bf6c7e2283 197 }
narshu 0:f3bf6c7e2283 198
narshu 0:f3bf6c7e2283 199 //*********************************************
narshu 0:f3bf6c7e2283 200 //
narshu 0:f3bf6c7e2283 201 // @Description converts encoder counts to distance in mm
narshu 0:f3bf6c7e2283 202 // @Parameters [encoder] (int) encoder counts
narshu 0:f3bf6c7e2283 203 // @returns distance in mm
narshu 0:f3bf6c7e2283 204 //
narshu 0:f3bf6c7e2283 205 //*********************************************
narshu 0:f3bf6c7e2283 206 int Motors::encoderToDistance(int encoder) {
narshu 0:f3bf6c7e2283 207 return int((float(encoder) / float(encoderRevCount)) * wheelmm);
narshu 0:f3bf6c7e2283 208 }
narshu 0:f3bf6c7e2283 209
narshu 0:f3bf6c7e2283 210 //*********************************************
narshu 0:f3bf6c7e2283 211 //
narshu 0:f3bf6c7e2283 212 // @Description converts distance in mm to encoder counts
narshu 0:f3bf6c7e2283 213 // @Parameters [distance] (int) distance in mm
narshu 0:f3bf6c7e2283 214 // @returns encoder counts
narshu 0:f3bf6c7e2283 215 //
narshu 0:f3bf6c7e2283 216 //*********************************************
narshu 0:f3bf6c7e2283 217 int Motors::distanceToEncoder(int distance) {
narshu 0:f3bf6c7e2283 218 return int((float(distance) / float(wheelmm)) * encoderRevCount);
narshu 0:f3bf6c7e2283 219 }
narshu 0:f3bf6c7e2283 220
narshu 0:f3bf6c7e2283 221 //*********************************************
narshu 0:f3bf6c7e2283 222 //
narshu 0:f3bf6c7e2283 223 // @Description number sign indicator. determines if number is positive or negative.
narshu 0:f3bf6c7e2283 224 // @Parameters [direction] (int) a number
narshu 0:f3bf6c7e2283 225 // @returns -1 if negative, 1 if positive
narshu 0:f3bf6c7e2283 226 //
narshu 0:f3bf6c7e2283 227 //*********************************************
narshu 0:f3bf6c7e2283 228 int Motors::getSignOfInt(int direction) {
narshu 0:f3bf6c7e2283 229
narshu 0:f3bf6c7e2283 230 if (direction > 0) {
narshu 0:f3bf6c7e2283 231 return 1;
narshu 0:f3bf6c7e2283 232 } else if (direction < 0) {
narshu 0:f3bf6c7e2283 233 return -1;
narshu 0:f3bf6c7e2283 234 }
narshu 0:f3bf6c7e2283 235 return -1;
narshu 0:f3bf6c7e2283 236 }
narshu 0:f3bf6c7e2283 237
narshu 0:f3bf6c7e2283 238 //*********************************************
narshu 0:f3bf6c7e2283 239 //Start of quarantined functions
narshu 0:f3bf6c7e2283 240
narshu 0:f3bf6c7e2283 241 void Motors::move(int distance, int speed) {
narshu 0:f3bf6c7e2283 242 //resetEncoders(); TODO use kalman as feedback instead!
narshu 0:f3bf6c7e2283 243
narshu 0:f3bf6c7e2283 244 int tempEndEncoder = 0;
narshu 0:f3bf6c7e2283 245 int startEncoderCount = 0;
narshu 0:f3bf6c7e2283 246
narshu 0:f3bf6c7e2283 247 tempEndEncoder = distanceToEncoder(abs(distance));
narshu 0:f3bf6c7e2283 248 startEncoderCount = getEncoder1();
narshu 0:f3bf6c7e2283 249
narshu 0:f3bf6c7e2283 250 setSpeed(getSignOfInt(distance) * speed);
narshu 0:f3bf6c7e2283 251
narshu 0:f3bf6c7e2283 252 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 0:f3bf6c7e2283 253 setSpeed(getSignOfInt(distance) * speed);
narshu 0:f3bf6c7e2283 254 }
narshu 0:f3bf6c7e2283 255
narshu 0:f3bf6c7e2283 256 //resetEncoders();
narshu 0:f3bf6c7e2283 257 setSpeed(0);
narshu 0:f3bf6c7e2283 258 }
narshu 0:f3bf6c7e2283 259
narshu 0:f3bf6c7e2283 260 void Motors::turn(int angle, int speed) {
narshu 0:f3bf6c7e2283 261 //resetEncoders(); TODO use kalman as feedback instead!
narshu 0:f3bf6c7e2283 262 int tempDistance = int((float(angle) / 360) * float(robotCircumference));
narshu 0:f3bf6c7e2283 263 int tempEndEncoder = 0;
narshu 0:f3bf6c7e2283 264 int startEncoderCount = 0;
narshu 0:f3bf6c7e2283 265
narshu 0:f3bf6c7e2283 266 tempEndEncoder = distanceToEncoder(abs(tempDistance));
narshu 0:f3bf6c7e2283 267 startEncoderCount = getEncoder1();
narshu 0:f3bf6c7e2283 268 setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
narshu 0:f3bf6c7e2283 269
narshu 0:f3bf6c7e2283 270 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 0:f3bf6c7e2283 271 setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
narshu 0:f3bf6c7e2283 272
narshu 0:f3bf6c7e2283 273 }
narshu 0:f3bf6c7e2283 274
narshu 0:f3bf6c7e2283 275 //resetEncoders();
narshu 0:f3bf6c7e2283 276 setSpeed(0);
narshu 0:f3bf6c7e2283 277 }
narshu 0:f3bf6c7e2283 278
narshu 0:f3bf6c7e2283 279 //Start of quarantined functions
narshu 0:f3bf6c7e2283 280 //*********************************************
narshu 0:f3bf6c7e2283 281
narshu 0:f3bf6c7e2283 282
narshu 0:f3bf6c7e2283 283 //*************************************************************************************
narshu 0:f3bf6c7e2283 284 // Private functions
narshu 0:f3bf6c7e2283 285 //*************************************************************************************
narshu 0:f3bf6c7e2283 286
narshu 0:f3bf6c7e2283 287 //*********************************************
narshu 0:f3bf6c7e2283 288 //
narshu 0:f3bf6c7e2283 289 // @Description internal set speed function
narshu 0:f3bf6c7e2283 290 // @Parameters speed1 min -127, max 127
narshu 0:f3bf6c7e2283 291 // @Parameters speed2 min -127, max 127
narshu 0:f3bf6c7e2283 292 //
narshu 0:f3bf6c7e2283 293 //*********************************************
narshu 0:f3bf6c7e2283 294 void Motors::_setSpeed(int speed1, int speed2) {
narshu 0:f3bf6c7e2283 295
narshu 0:f3bf6c7e2283 296 //set global encoder values
narshu 0:f3bf6c7e2283 297 _lastEncoder1 = getEncoder1();
narshu 0:f3bf6c7e2283 298 _lastEncoder2 = getEncoder2();
narshu 0:f3bf6c7e2283 299
narshu 0:f3bf6c7e2283 300 //Speed ranges from -127 to 127
narshu 0:f3bf6c7e2283 301 if (speed1 > 0) {
narshu 0:f3bf6c7e2283 302 //Motor1 forwards
narshu 0:f3bf6c7e2283 303 Motor1Enable = (float)speed1/127;
narshu 0:f3bf6c7e2283 304 Motor1A = 1;
narshu 0:f3bf6c7e2283 305 Motor1B = 0;
narshu 0:f3bf6c7e2283 306 //pwm the h bridge driver range 0 to 1 type float.
narshu 0:f3bf6c7e2283 307
narshu 0:f3bf6c7e2283 308 } else if (speed1 < 0) {
narshu 0:f3bf6c7e2283 309 //Motor1 backwards
narshu 0:f3bf6c7e2283 310 Motor1Enable = (float)abs(speed1)/127;
narshu 0:f3bf6c7e2283 311 Motor1A = 0;
narshu 0:f3bf6c7e2283 312 Motor1B = 1;
narshu 0:f3bf6c7e2283 313
narshu 0:f3bf6c7e2283 314 } else if (speed1 ==0) {
narshu 0:f3bf6c7e2283 315 _stop(1,0);
narshu 0:f3bf6c7e2283 316 }
narshu 0:f3bf6c7e2283 317
narshu 0:f3bf6c7e2283 318 if (speed2 > 0) {
narshu 0:f3bf6c7e2283 319 //Motor2 forwards
narshu 0:f3bf6c7e2283 320 Motor2Enable = (float)speed2/127;
narshu 0:f3bf6c7e2283 321
narshu 0:f3bf6c7e2283 322 Motor2A = 1;
narshu 0:f3bf6c7e2283 323 Motor2B = 0;
narshu 0:f3bf6c7e2283 324
narshu 0:f3bf6c7e2283 325 } else if (speed2 < 0) {
narshu 0:f3bf6c7e2283 326 //Motor2 backwards
narshu 0:f3bf6c7e2283 327 Motor2Enable = (float)abs(speed2)/127;
narshu 0:f3bf6c7e2283 328 Motor2A = 0;
narshu 0:f3bf6c7e2283 329 Motor2B = 1;
narshu 0:f3bf6c7e2283 330 } else if (speed2 == 0) {
narshu 0:f3bf6c7e2283 331 _stop(0,1);
narshu 0:f3bf6c7e2283 332 }
narshu 0:f3bf6c7e2283 333
narshu 0:f3bf6c7e2283 334 }
narshu 0:f3bf6c7e2283 335
narshu 0:f3bf6c7e2283 336
narshu 0:f3bf6c7e2283 337 //*********************************************
narshu 0:f3bf6c7e2283 338 //
narshu 0:f3bf6c7e2283 339 // @Description stop command for both motors
narshu 0:f3bf6c7e2283 340 //
narshu 0:f3bf6c7e2283 341 //*********************************************
narshu 0:f3bf6c7e2283 342 void Motors::_stop() {
narshu 0:f3bf6c7e2283 343 _stop(1,1);
narshu 0:f3bf6c7e2283 344 }
narshu 0:f3bf6c7e2283 345
narshu 0:f3bf6c7e2283 346 //*********************************************
narshu 0:f3bf6c7e2283 347 //
narshu 0:f3bf6c7e2283 348 // @Description stop command for individual motors
narshu 0:f3bf6c7e2283 349 // @Parameter [motor1] stops motor1. =1 is stop. =0 do nothing
narshu 0:f3bf6c7e2283 350 // @Parameter [motor2] stops motor2. =1 is stop. =0 do nothing
narshu 0:f3bf6c7e2283 351 //
narshu 0:f3bf6c7e2283 352 //*********************************************
narshu 0:f3bf6c7e2283 353 void Motors::_stop(int motor1, int motor2) {
narshu 0:f3bf6c7e2283 354 if (motor1 == 1) {
narshu 0:f3bf6c7e2283 355 Motor1Enable = 1;
narshu 0:f3bf6c7e2283 356 Motor1A = 0;
narshu 0:f3bf6c7e2283 357 Motor1B = 0;
narshu 0:f3bf6c7e2283 358 }
narshu 0:f3bf6c7e2283 359
narshu 0:f3bf6c7e2283 360 if (motor2 == 1) {
narshu 0:f3bf6c7e2283 361 Motor2Enable = 1;
narshu 0:f3bf6c7e2283 362 Motor2A = 0;
narshu 0:f3bf6c7e2283 363 Motor2B = 0;
narshu 0:f3bf6c7e2283 364 }
narshu 0:f3bf6c7e2283 365
narshu 0:f3bf6c7e2283 366 }
narshu 0:f3bf6c7e2283 367
narshu 0:f3bf6c7e2283 368 //*************************************************************************************
narshu 0:f3bf6c7e2283 369 // Redundant functions
narshu 0:f3bf6c7e2283 370 //*************************************************************************************
narshu 0:f3bf6c7e2283 371
narshu 0:f3bf6c7e2283 372 //Redudant
narshu 0:f3bf6c7e2283 373 void Motors::setMode(int mode) {
narshu 0:f3bf6c7e2283 374 }
narshu 0:f3bf6c7e2283 375
narshu 0:f3bf6c7e2283 376 //Redudant
narshu 0:f3bf6c7e2283 377 void Motors::sendCommand(char command) {
narshu 0:f3bf6c7e2283 378 }
narshu 0:f3bf6c7e2283 379
narshu 0:f3bf6c7e2283 380 //Redudant
narshu 0:f3bf6c7e2283 381 void Motors::sendCommand(char command1, char command2 ) {
narshu 0:f3bf6c7e2283 382 }