153 lines
3.5 KiB
C++
153 lines
3.5 KiB
C++
|
|
/**
|
|
* Prototype
|
|
* Arduino Uno
|
|
* L298N
|
|
**/
|
|
|
|
#define Apos 9
|
|
#define Aneg 10
|
|
|
|
#define Bpos 5
|
|
#define Bneg 6
|
|
|
|
volatile int Aspeed = 255;
|
|
volatile int Bspeed = 255;
|
|
|
|
volatile boolean Arunning = false;
|
|
volatile boolean Brunning = false;
|
|
|
|
void setup() {
|
|
Serial.begin(57600);
|
|
Serial.println("Started takeup demo");
|
|
pinMode(Apos, OUTPUT);
|
|
pinMode(Aneg, OUTPUT);
|
|
pinMode(Bpos, OUTPUT);
|
|
pinMode(Bneg, OUTPUT);
|
|
}
|
|
|
|
|
|
void loop() {
|
|
clockwise(0);
|
|
counter_clockwise(1);
|
|
set_speed(0, 255);
|
|
set_speed(1, 255);
|
|
delay(5000);
|
|
brake();
|
|
delay(2000);
|
|
counter_clockwise(0);
|
|
clockwise(1);
|
|
set_speed(0, 150);
|
|
set_speed(1, 150);
|
|
delay(5000);
|
|
brake();
|
|
delay(2000);
|
|
}
|
|
|
|
//input value from 0 to 255
|
|
void set_speed (int motor, int val){
|
|
if (motor == 0) {
|
|
Aspeed = val;
|
|
} else if (motor == 1) {
|
|
Bspeed = val;
|
|
}
|
|
}
|
|
|
|
void clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
|
|
int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
|
|
int stepMs = round((float) ms / (float) steps);
|
|
int motorSpeed = startSpeed;
|
|
for (int i = 0; i < steps; i++) {
|
|
if (motor == 0) {
|
|
analogWrite(Apos, motorSpeed);
|
|
analogWrite(Aneg, 0);
|
|
Arunning = true;
|
|
} else if (motor == 1) {
|
|
analogWrite(Bpos, motorSpeed);
|
|
analogWrite(Bneg, 0);
|
|
Brunning = true;
|
|
}
|
|
delay(stepMs);
|
|
motorSpeed += stopSpeed > startSpeed ? 1 : -1;
|
|
}
|
|
if (motor == 0) {
|
|
Aspeed = stopSpeed;
|
|
} else if (motor == 1) {
|
|
Bspeed = stopSpeed;
|
|
}
|
|
}
|
|
|
|
void counter_clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
|
|
int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
|
|
int stepMs = round((float) ms / (float) steps);
|
|
int motorSpeed = startSpeed;
|
|
if (motor == 0) {
|
|
analogWrite(Apos, 0);
|
|
} else if (motor == 1) {
|
|
analogWrite(Bpos, 0);
|
|
}
|
|
for (int i = 0; i < steps; i++) {
|
|
if (motor == 0) {
|
|
analogWrite(Aneg, motorSpeed);
|
|
Arunning = true;
|
|
} else if (motor == 1) {
|
|
analogWrite(Bneg, motorSpeed);
|
|
Brunning = true;
|
|
}
|
|
delay(stepMs);
|
|
motorSpeed += stopSpeed > startSpeed ? 1 : -1;
|
|
}
|
|
if (motor == 0) {
|
|
Aspeed = stopSpeed;
|
|
} else if (motor == 1) {
|
|
Bspeed = stopSpeed;
|
|
}
|
|
}
|
|
|
|
void clockwise (int motor){
|
|
if (motor == 0) {
|
|
analogWrite(Apos, Aspeed);
|
|
analogWrite(Aneg, 0);
|
|
Arunning = true;
|
|
} else if (motor == 1) {
|
|
analogWrite(Bpos, Bspeed);
|
|
analogWrite(Bneg, 0);
|
|
Brunning = true;
|
|
}
|
|
}
|
|
|
|
void counter_clockwise (int motor){
|
|
if (motor == 0) {
|
|
analogWrite(Apos, 0);
|
|
analogWrite(Aneg, Aspeed);
|
|
Arunning = true;
|
|
} else if (motor == 1) {
|
|
analogWrite(Bpos, 0);
|
|
analogWrite(Bneg, Bspeed);
|
|
Brunning = true;
|
|
}
|
|
}
|
|
|
|
void brake (){
|
|
if (Arunning) {
|
|
digitalWrite(Apos, LOW);
|
|
digitalWrite(Aneg, LOW);
|
|
Arunning = false;
|
|
}
|
|
if (Brunning) {
|
|
digitalWrite(Bpos, LOW);
|
|
digitalWrite(Bneg, LOW);
|
|
Brunning = false;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Vex = Higher voltage that can power the motor and/or the shield as well as the Arduino if wanted
|
|
* 5V = Must be a clean 5V that can power the Arduino and shield, and if wanted also the motor itself
|
|
* GND = Ass always, the ground of everything
|
|
* B- = Connection of the first motor or first winding of the stepper motor
|
|
* B+ = Connection of the first motor or first winding of the stepper motor
|
|
* A- = Connection of the second motor or second winding of the stepper motor
|
|
* A+ = Connection of the second motor or second winding of the stepper motor
|
|
**/
|