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
}
Ejemplo n.º 2
0
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
}
Ejemplo n.º 3
0
void initialize() {
 //initialize
    pinMode(WHITELINE_PIN, WHITELINE_DIR);
    pinMode(ODOMETRY_PIN, ODOMETRY_DIR);
    pinMode(EXTINGUISHER_PIN, EXTINGUISHER_DIR);
//	pinMode(FRONT_PROXIMITY_PIN, FRONT_PROXIMITY_DIR);
    AFMS.begin();  // create with the default frequency 1.6KHz
}
Ejemplo n.º 4
0
void Stage::begin()
{
  //Initiliaze the motor shield
  lower_afms.begin();
  upper_afms.begin();

  //Setup pins for input
  pinMode(Z_ULIMIT_SWITCH, INPUT_PULLUP);
  pinMode(Z_LLIMIT_SWITCH, INPUT_PULLUP);


  _x_pos = 0;
  _y_pos = 0;
  _z_pos = 0;

  _xy_interval = 15;
  _z_interval = 15;

}
Ejemplo n.º 5
0
/**
 * First thing this machine does on startup.  Runs only once.
 */
void setup() {
  Serial.begin(BAUD);  // open coms
  
#if MOTOR_SHIELD_VERSION == 2
  AFMS.begin();  // create with the default frequency 1.6KHz
#endif

  help();  // say hello
  position(0,0);  // set staring position
  feedrate(200);  // set default speed
  ready();
}
Ejemplo n.º 6
0
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);
  
}
Ejemplo n.º 7
0
Archivo: src.cpp Proyecto: ttrask/unf
void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");

  AFMS.begin();  // create with the default frequency 1.6KHz
  //AFMS.begin(1000);  // OR with a different frequency, say 1KHz
  
  // Set the speed to start, from 0 (off) to 255 (max speed)
  myMotor->setSpeed(150);
  myOtherMotor->setSpeed(150);
  myMotor->run(FORWARD);
  myOtherMotor->run(FORWARD);

// turn on motor
  myMotor->run(RELEASE);
  myOtherMotor->run(RELEASE);

}
Ejemplo n.º 8
0
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
    
}
Ejemplo n.º 9
0
void setup() {
    AFMS.begin();
    setup_motors();
    setup_distance_sensor();
}