takeup/ino/demo/demo.ino

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
**/