72 lines
1.3 KiB
C++
72 lines
1.3 KiB
C++
#include <Motion.h>
|
|
#include <Motor.h>
|
|
|
|
/*
|
|
|
|
Two motion objects are available from the MMM Motions library
|
|
(1) MotionA
|
|
(2) MotionB
|
|
|
|
MOTION {
|
|
MOTIONA ,
|
|
MOTIONB1
|
|
};
|
|
|
|
Each objects needs intialisation with a specific input
|
|
from which ADC reading will be performed.
|
|
|
|
Users may choose and identify inputs aa INPUT[A0, ... ,A7].
|
|
ex: MotionA.init(INPOUTA5);
|
|
|
|
INPUT {
|
|
INPUTA0,
|
|
INPUTA1,
|
|
INPUTA2,
|
|
INPUTA3,
|
|
INPUTA4,
|
|
INPUTA5,
|
|
INPUTA6,
|
|
INPUTA7
|
|
}
|
|
|
|
All calculation of physics (position, velocity and acceleration)
|
|
are computed internally usign interrupts and timers. To access
|
|
such information, user may call getPosition(), getVelocity()
|
|
and getAcceleration() respectively on a initialised Motion object.
|
|
|
|
int getPosition();
|
|
float getVelocity();
|
|
float getAcceleration();
|
|
|
|
*/
|
|
|
|
void setup() {
|
|
MotionA.init(INPUTA0);
|
|
MotorA.init();
|
|
Serial.begin(9600);
|
|
|
|
MotionA.k = 0.2f;
|
|
MotionA.m = 0.3f;
|
|
MotionA.d = 0.02f;
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
if(MotionA.F < 0) MotorA.direction(FORWARD);
|
|
else MotorA.direction(BACKWARD);
|
|
|
|
float t = abs(MotionA.F);
|
|
MotorA.torque(t);
|
|
|
|
/*
|
|
Serial.print("position: "); Serial.println(MotionA.getPosition());
|
|
Serial.print("velocity: "); Serial.println(MotionA.getVelocity());
|
|
Serial.print("accel: "); Serial.println(MotionA.getAcceleration());
|
|
Serial.println("-------");
|
|
delay(100);
|
|
*/
|
|
}
|
|
|
|
|