void BackSeatDriver_DCMotorAdapter::attach() {
_AFMS = Adafruit_MotorShield();
_AFMS.begin();
for (short i = 0; i < _numMotors; i++) {
_motors[i] = _AFMS.getMotor(abs(_motorMap[i]));
}
}
void MyBotMotors::Slide(int dir,int spee)
{
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motorBR = AFMS.getMotor(1);
Adafruit_DCMotor *motorBL = AFMS.getMotor(2);
Adafruit_DCMotor *motorFL = AFMS.getMotor(3);
Adafruit_DCMotor *motorFR = AFMS.getMotor(4);
motorFL->setSpeed(spee);
motorFR->setSpeed(spee);
motorBL->setSpeed(spee);
motorBR->setSpeed(spee);
//Right for dir = 1, Left for dir = 0
if (dir == 1)
{
motorFL->run(BACKWARD);
motorFR->run(FORWARD);
motorBL->run(FORWARD);
motorBR->run(BACKWARD);
}
else
{
motorFL->run(FORWARD);
motorFR->run(BACKWARD);
motorBL->run(BACKWARD);
motorBR->run(FORWARD);
}
}
void MyBotMotors::Drive(int dir,int spee)
{
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motorBR = AFMS.getMotor(1);
Adafruit_DCMotor *motorBL = AFMS.getMotor(2);
Adafruit_DCMotor *motorFL = AFMS.getMotor(3);
Adafruit_DCMotor *motorFR = AFMS.getMotor(4);
motorFL->setSpeed(spee);
motorFR->setSpeed(spee);
motorBL->setSpeed(spee);
motorBR->setSpeed(spee);
// FWD for dir = 1, BCKWD for dir = 0
// Potentiall include an if loop for if the input speed is too slow to give motor 6V
if (dir == 1)
{
motorFL->run(FORWARD);
motorFR->run(FORWARD);
motorBL->run(FORWARD);
motorBR->run(FORWARD);
} else
{
motorFL->run(BACKWARD);
motorFR->run(BACKWARD);
motorBL->run(BACKWARD);
motorBR->run(BACKWARD);
}
}
Motor::Motor() {
AFMS = Adafruit_MotorShield();
myMotor1 = AFMS.getMotor(1);
myMotor2 = AFMS.getMotor(2);
myMotor3 = AFMS.getMotor(3);
myMotor4 = AFMS.getMotor(4);
}
void MyBotMotors::Stop()
{
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motorBR = AFMS.getMotor(1);
Adafruit_DCMotor *motorBL = AFMS.getMotor(2);
Adafruit_DCMotor *motorFL = AFMS.getMotor(3);
Adafruit_DCMotor *motorFR = AFMS.getMotor(4);
motorFL->run(RELEASE);
motorFR->run(RELEASE);
motorBL->run(RELEASE);
motorBR->run(RELEASE);
}
void stepper_init() {
// Create the motor shield object with the default I2C address
AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);
// create with the default frequency 1.6KHz
AFMS.begin();
// motor port #1 M1 and M2
myStepper1 = AFMS.getStepper(sstvars.stepsPerRotation, 1);
// motor port #2 (M3 and M4)
//*myStepper1 = AFMS.getStepper(sstvars.stepsPerRotation, 2);
}
void MyBotMotors::Turn(int dir)
{
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *motorBR = AFMS.getMotor(1);
Adafruit_DCMotor *motorBL = AFMS.getMotor(2);
Adafruit_DCMotor *motorFL = AFMS.getMotor(3);
Adafruit_DCMotor *motorFR = AFMS.getMotor(4);
//Right for dir = 1, Left for dir = 0
if (dir == 1)
{
motorFL->run(FORWARD);
motorFR->run(FORWARD);
motorBL->run(FORWARD);
motorBR->run(FORWARD);
}
else
{
motorFL->run(FORWARD);
motorFR->run(FORWARD);
motorBL->run(FORWARD);
motorBR->run(FORWARD);
}
}
//------------------------------------------------------------------------------ // GLOBALS //------------------------------------------------------------------------------ // Initialize Adafruit stepper controller // Connect stepper motors with 400 steps per revolution (1.8 degree) #if MOTOR_SHIELD_VERSION == 1 static AF_Stepper m1(STEPS_PER_TURN, 1); // to motor port #1 (M1 and M2) static AF_Stepper m2(STEPS_PER_TURN, 2); // to motor port #2 (M3 and M4) #endif #if MOTOR_SHIELD_VERSION == 2 // Create the motor shield object with the default I2C address Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); Adafruit_StepperMotor *m1 = AFMS.getStepper(STEPS_PER_TURN, 1); // to motor port #1 (M1 and M2) Adafruit_StepperMotor *m2 = AFMS.getStepper(STEPS_PER_TURN, 2); // to motor port #2 (M3 and M4) #endif char buffer[MAX_BUF]; // where we store the message until we get a ';' int sofar; // how much is in the buffer float px, py; // location // speeds float fr=0; // human version // machine version long step_delay_ms; long step_delay_us;
/*
This is a test sketch for the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control
For use with the Adafruit Motor Shield v2
----> http://www.adafruit.com/products/1438
*/
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);
// Connect a stepper motor with 200 steps per revolution (1.8 degree)
// to motor port #2 (M3 and M4)
Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 2);
void setup() {
Serial.begin(9600); // set up Serial library at 9600 bps
AFMS.begin(); // create with the default frequency 1.6KHz
//AFMS.begin(1000); // OR with a different frequency, say 1KHz
myMotor->setSpeed(10); // 10 rpm
#include <cstdio>
#include "Adafruit_MotorShield/Adafruit_MotorShield.h"
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(10, 0x60);
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);
// Connect a stepper motor with 200 steps per revolution (1.8 degree)
// to motor port #2 (M3 and M4)
Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 2);
void setup() {
printf("Stepper Test\n");
AFMS.begin(); // create with the default frequency 1.6KHz
//AFMS.begin(1000); // OR with a different frequency, say 1KHz
myMotor->setSpeed(10); // 10 rpm
}
void loop() {
printf("Single coil steps\n");
myMotor->step(100, FORWARD, SINGLE);
myMotor->step(100, BACKWARD, SINGLE);
printf("Double coil steps\n");
myMotor->step(100, FORWARD, DOUBLE);
myMotor->step(100, BACKWARD, DOUBLE);
int main()
{
//INITIALIZE STEPPER MOTOR
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 1);
AFMS.begin(2400); // create with the frequency 2.4KHz
myMotor->setSpeed(1000); // rpm (this has an upper limit way below 1000 rpm)
myMotor->release();
//SET UP SOME VARIABLES
double ro[3];
double vo[3];
double recef[3];
double vecef[3];
char typerun, typeinput, opsmode;
gravconsttype whichconst;
double sec, secC, jd, jdC, tsince, startmfe, stopmfe, deltamin;
double tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2;
double latlongh[3]; //lat, long in rad, h in km above ellipsoid
double siteLat, siteLon, siteAlt, siteLatRad, siteLonRad;
double razel[3];
double razelrates[3];
int year; int mon; int day; int hr; int min;
int yearC; int monC; int dayC; int hrC; int minC;
typedef char str3[4];
str3 monstr[13];
elsetrec satrec;
double steps_per_degree = 1.38889; //Stepper motor steps per degree azimuth
float elevation;
//VARIABLES FOR STEPPER CALCULATIONS
float azimuth; //-180 to 0 to 180
float prevAzimuth;
float cAzimuth; //From 0 to 359.99
float prevcAzimuth;
bool stepperRelative = 0; //Has the stepper direction been initialized?
float azimuthDatum = 0;
int stepsFromDatum = 0;
int stepsNext = 0;
int dirNext = 1;
int totalSteps = 0;
int prevDir = 3; //Initialize at 3 to indicate that there is no previous direction yet (you can't have a "previous direction" until the third step)
double azError = 0;
//SET REAL TIME CLOCK (Set values manually using custom excel function until I find a way to do it automatically)
set_time(1440763200);
//SET VARIABLES
opsmode = 'i';
typerun = 'c';
typeinput = 'e';
whichconst = wgs72;
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
strcpy(monstr[1], "Jan");
strcpy(monstr[2], "Feb");
strcpy(monstr[3], "Mar");
strcpy(monstr[4], "Apr");
strcpy(monstr[5], "May");
strcpy(monstr[6], "Jun");
strcpy(monstr[7], "Jul");
strcpy(monstr[8], "Aug");
strcpy(monstr[9], "Sep");
strcpy(monstr[10], "Oct");
strcpy(monstr[11], "Nov");
strcpy(monstr[12], "Dec");
//ENTER TWO-LINE ELEMENT HERE
char longstr1[] = "1 25544U 98067A 15239.40934558 .00012538 00000-0 18683-3 0 9996";
char longstr2[] = "2 25544 51.6452 88.4122 0001595 95.9665 324.8493 15.55497522959124";
//ENTER SITE DETAILS HERE
siteLat = 30.25; //+North (Austin)
siteLon = -97.75; //+East (Austin)
siteAlt = 0.15; //km (Austin)
siteLatRad = siteLat * pi / 180.0;
siteLonRad = siteLon * pi / 180.0;
//FREEDOM OF MOVEMENT CHECKS
for (int i = 0; i < 500; i = i + 10) {
EL_SERVO = Convert_El_to_Servo(-90.0 + 180.0 * i / 500.0);
}
wait(1);
for (int i = 500; i > 0; i = i - 10) {
EL_SERVO = Convert_El_to_Servo(-90.0 + 180.0 * i / 500.0);
}
wait(1);
/*
//FREEDOM OF MOVEMENT CHECKS STEPPER
myMotor->step(500, FORWARD, SINGLE);
myMotor->step(500, BACKWARD, SINGLE);
*/
//INITIALIZE SATELLITE TRACKING
//pc.printf("Initializing satellite orbit...\n");
twoline2rv(longstr1, longstr2, typerun, typeinput, opsmode, whichconst, startmfe, stopmfe, deltamin, satrec );
//pc.printf("twoline2rv function complete...\n");
//Call propogator to get initial state vector value
sgp4(whichconst, satrec, 0.0, ro, vo);
//pc.printf("SGP4 at t = 0 to get initial state vector complete...\n");
jd = satrec.jdsatepoch;
invjday(jd, year, mon, day, hr, min, sec);
pc.printf("Scenario Epoch %3i %3s%5i%3i:%2i:%12.9f \n", day, monstr[mon], year, hr, min, sec);
jdC = getJulianFromUnix(time(NULL));
invjday( jdC, yearC, monC, dayC, hrC, minC, secC);
pc.printf("Current Time %3i %3s%5i%3i:%2i:%12.9f \n", dayC, monstr[monC], yearC, hrC, minC, secC);
//pc.printf(" Time PosX PosY PosZ Vx Vy Vz\n");
//pc.printf(" Time Lat Long Height Range Azimuth Elevation\n");
//BEGIN SATELLITE TRACKING
while(1)
{
//RUN SGP4 AND COORDINATE TRANSFORMATION COMPUTATIONS
jdC = getJulianFromUnix(time(NULL));
tsince = (jdC - jd) * 24.0 * 60.0;
sgp4(whichconst, satrec, tsince, ro, vo);
teme2ecef(ro, vo, jdC, recef, vecef);
ijk2ll(recef, latlongh);
rv2azel(ro, vo, siteLatRad, siteLonRad, siteAlt, jdC, razel, razelrates);
//CHECK FOR ERRORS
if (satrec.error > 0)
{
pc.printf("# *** error: t:= %f *** code = %3d\n", satrec.t, satrec.error);
}
else
{
azimuth = razel[1]*180/pi;
if (azimuth < 0) {
cAzimuth = 360.0 + azimuth;
}
else {
cAzimuth = azimuth;
}
elevation = razel[2]*180/pi;
//pc.printf("%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f\n", satrec.t, recef[0], recef[1], recef[2], vecef[0], vecef[1], vecef[2]);
//pc.printf("%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f%16.8f\n", satrec.t, latlongh[0]*180/pi, latlongh[1]*180/pi, latlongh[2], razel[0], razel[1]*180/pi, razel[2]*180/pi);
//For first step, initialize the stepper direction assuming its initial position is true north
if (stepperRelative == 0){
stepsNext = int(cAzimuth * steps_per_degree);
dirNext = 2;
myMotor->step(stepsNext, dirNext, MICROSTEP); //Turn stepper clockwise to approximate initial azimuth
stepperRelative = 1;
azimuthDatum = stepsNext / steps_per_degree;
prevAzimuth = azimuth;
prevcAzimuth = cAzimuth;
pc.printf(" Azimuth Azimuth Datum Steps from Datum Total Steps Steps Next Direction Az. Error\n");
}
else {
//Determine direction of rotation (note this will be incorrect if azimuth has crossed true north since previous step - this is dealt with later)
if ( cAzimuth < prevcAzimuth ) {
dirNext = 1; //CCW
}
else {
dirNext = 2; //CW
}
//Check if azimuth has crossed from 360 to 0 degrees or vice versa
if (abs( (azimuth - prevAzimuth) - (cAzimuth - prevcAzimuth) ) > 0.0001) {
//Recalculate direction of rotation
if ( cAzimuth > prevcAzimuth ) {
dirNext = 1; //CCW
}
else {
dirNext = 2; //CW
}
//Reset the azimuth datum
if (dirNext == 1) {
azimuthDatum = cAzimuth + azError + prevcAzimuth;
}
else {
azimuthDatum = cAzimuth - azError + (prevcAzimuth - 360);
}
//Reset totalSteps
totalSteps = 0;
}
//Check if azimuth rate has changed directions
if (prevDir != 3) { //prevDir of 3 means there is no previous direction yet
if (prevDir != dirNext) {
//Reset totalSteps
totalSteps = 0;
//Reset azimuth datum
if (dirNext == 1) {
azimuthDatum = prevcAzimuth + azError;
}
else {
azimuthDatum = prevcAzimuth - azError;
}
}
}
stepsFromDatum = int( abs(cAzimuth - azimuthDatum) * steps_per_degree );
stepsNext = stepsFromDatum - totalSteps;
totalSteps += stepsNext;
azError = abs(cAzimuth - azimuthDatum) - (totalSteps / steps_per_degree);
pc.printf("%20.2f%20.2f%20d%20d%20d%20d%20.2f\n", cAzimuth, azimuthDatum, stepsFromDatum, totalSteps, stepsNext, dirNext, azError);
if (stepsNext > 250) {
pc.printf("something's probably wrong... too many steps\n\n\n\n");
while(1){} // pause
}
myMotor->step(stepsNext, dirNext, MICROSTEP);
}
EL_SERVO = Convert_El_to_Servo(elevation);
prevAzimuth = azimuth;
prevcAzimuth = cAzimuth;
prevDir = dirNext;
}
wait(1);
} //indefinite loop
}
/*
Stage.cpp - Library for controlling the stage on the OpenLabTools microscope
Written by James Ritchie for OpenLabTools
github.com/OpenLabTools/Microscope
*/
#include "Arduino.h"
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "Stage.h"
#include "TouchScreen.h"
// Create the motor shield objects
Adafruit_MotorShield lower_afms = Adafruit_MotorShield(0x60);
Adafruit_MotorShield upper_afms = Adafruit_MotorShield(0x61);
// Create stepper motors for each axis (200 steps per rev)
Adafruit_StepperMotor *xy_a_motor = lower_afms.getStepper(200,1);
Adafruit_StepperMotor *xy_b_motor = lower_afms.getStepper(200,2);
Adafruit_StepperMotor *z_1_motor = upper_afms.getStepper(200, 1);
Adafruit_StepperMotor *z_2_motor = upper_afms.getStepper(200, 2);
//Create the touchscreen object
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
Stage::Stage() {
//Initialize AccelStepper object with wrapper functions and parameters.
calibrated = false;
}
void Stage::begin()
