Stepper robot example from today’s class

//
// stepperbot.c - Stepper robot example from class
// Left motor is on RD-3, right motor is on RE0-3
//
// Written by Ted Burke - last updated 17-11-2015
//

#include <xc.h>
#include <libpic30.h>
 
// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

void forward(float);
void turn_left(float);
void turn_right(float);
void left_step_forward();
void left_step_back();
void right_step_forward();
void right_step_back();

const float mm_per_step = 0.2356;
const float degrees_per_step = 0.225;

int main()
{
    // Set up which pins are which
    TRISD = 0b1111111111110000;
    TRISE = 0b1111111111110000;
    
    while(1)
    {
        // Forward around half octagon
        forward(480);
        turn_right(45);
        forward(480);
        turn_right(45);
        forward(480);
        turn_right(45);
        forward(480);
        turn_right(180); // U turn
        
        // Back to beginning
        forward(480);
        turn_left(45);
        forward(480);
        turn_left(45);
        forward(480);
        turn_left(45);
        forward(480);
        turn_left(180); // U turn
    }

    return 0;
}

void forward(float millimetres)
{
    int steps = millimetres / mm_per_step;
    while(steps > 0)
    {
        left_step_forward();
        right_step_forward();
        __delay32(60000);
        steps = steps - 1;
    }
}

void turn_left(float degrees)
{
    int steps = degrees / degrees_per_step;
    while(steps > 0)
    {
        left_step_back();
        right_step_forward();
        __delay32(60000);
        steps = steps - 1;
    }
}

void turn_right(float degrees)
{
    int steps = degrees / degrees_per_step;
    while(steps > 0)
    {
        left_step_forward();
        right_step_back();
        __delay32(60000);
        steps = steps - 1;
    }
}

void left_step_forward()
{
    if      (LATD == 0b0001) LATD = 0b0010;
    else if (LATD == 0b0010) LATD = 0b0100;
    else if (LATD == 0b0100) LATD = 0b1000;
    else if (LATD == 0b1000) LATD = 0b0001;
    else LATD = 0b0001;
}

void left_step_back()
{
    if      (LATD == 0b0001) LATD = 0b1000;
    else if (LATD == 0b0010) LATD = 0b0001;
    else if (LATD == 0b0100) LATD = 0b0010;
    else if (LATD == 0b1000) LATD = 0b0100;
    else LATD = 0b0001;
}

void right_step_forward()
{
    if      (LATE == 0b0001) LATE = 0b0010;
    else if (LATE == 0b0010) LATE = 0b0100;
    else if (LATE == 0b0100) LATE = 0b1000;
    else if (LATE == 0b1000) LATE = 0b0001;
    else LATE = 0b0001;
}
void right_step_back()
{
    if      (LATE == 0b0001) LATE = 0b1000;
    else if (LATE == 0b0010) LATE = 0b0001;
    else if (LATE == 0b0100) LATE = 0b0010;
    else if (LATE == 0b1000) LATE = 0b0100;
    else LATE = 0b0001;
}
Advertisements
This entry was posted in Uncategorized. Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s