Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

motors/motors.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
23:1901cb6d0d95 
Eurobot 2012 Primary;

Who changed what in which revision?

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