Musical-Robotics/MR_Motor.cpp at master · mtderryberry/Musical-Robotics · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
//
//  MR_Motor.cpp
//
//
//  Created by Rayce Stipanovich on 11/1/12.
//
//

#include "MR_Motor.h"
bool DRIVE_MOTOR = false;

void MotorSetup() {
    pinMode(MR_MOTOR_PWM_PIN, OUTPUT);
    pinMode(MR_MOTOR_F_PIN, OUTPUT);
    pinMode(MR_MOTOR_R_PIN, OUTPUT);
	digitalWrite(MR_MOTOR_F_PIN, LOW);
	digitalWrite(MR_MOTOR_R_PIN, LOW);
	digitalWrite(MR_MOTOR_PWM_PIN, LOW);
}

void velocityControlMotor(int velocity, bool direction) {
	//direction assign
	if (direction) {
		digitalWrite(MR_MOTOR_F_PIN, HIGH);
		digitalWrite(MR_MOTOR_R_PIN, LOW);
	} else {
		digitalWrite(MR_MOTOR_F_PIN, LOW);
		digitalWrite(MR_MOTOR_R_PIN, HIGH);
	}

    //keep velocity within motor constraints
	velocity = constrain(velocity, 0, 127);
	velocity = map(velocity, 0, 127, 0, 255);

	//now assign speed control
    if (DRIVE_MOTOR) {
        analogWrite(MR_MOTOR_PWM_PIN, velocity);
    } else {
		analogWrite(MR_MOTOR_F_PIN, 0);
	}
}

//turn motor on
void EnableMotor() {
    DRIVE_MOTOR = true;
}

//turn motor off
void DisableMotor() {
    DRIVE_MOTOR = false;
}