SCARA fixed sequence example code

//
// SCARA robot manipulator fixed sequence example for dsPIC30F4011
// written by Ted Burke - last updated 19-10-2015
//

#include <xc.h>
#include <math.h>
#include <libpic30.h>

#define PI 3.1415926535897

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Fosc=16x7.5MHz, i.e. 30 MIPS
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

double L1 = 0.25;             // 25 cm
double L2 = 0.20;             // 20 cm
double pdc_0_degrees = 1.0;   // 1.0 ms gives 0 degrees
double pdc_180_degrees = 2.0; // 2.0 ms gives 180 degrees

// Define a fixed sequence of target coordinates
double T_step = 1.0; // moves point every 1 second
int N = 4; // number of points
double x[] = {10.0, 12.0, 15.0, 19.0};
double y[] = {12.0, 8.0, 13.0, 13.0};

// function prototypes
void move_to(double x, double y);
void setup();

int main(void)
{
    setup();
    
    // Move through fixed sequence
    int n = 0;
    while(1)
    {
        move_to(x[n], y[n]);
        __delay32(T_step * 30000000L);
        n = n + 1;
        if (n >= 4) n = 0;
    }
}

void move_to(double x, double y)
{
    double S, E;
    
    // inverse kinematic equations
    E = acos( (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2) );
    S = atan2(y, x) - acos ( (x*x + y*y + L1*L1 - L2*L2) / (2*L1*sqrt(x*x+y*y)) );
    
    // Set servo angles
    PDC1 = pdc_0_degrees + (pdc_180_degrees - pdc_0_degrees) * S / PI;
    PDC2 = pdc_0_degrees + (pdc_180_degrees - pdc_0_degrees) * E / PI;
}

void setup()
{
    // Configure PWM
    // PWM period = PTPER * prescale * Tcy = 9470 * 64 * 33.33ns = 20ms
    _PMOD1 = 0;   // PWM channel 1 mode: 0 for complementary, 1 for independent
    _PMOD2 = 0;   // PWM channel 2 mode: 0 for complementary, 1 for independent
    _PEN1H = 1;   // PWM1H pin enable: 1 to enable, 0 to disable
    _PEN2H = 1;   // PWM2H pin enable: 1 to enable, 0 to disable
    _PTCKPS = 3;  // PWM prescaler setting: 0=1:1, 1=1:4, 2=1:16, 3=1:64
    PTPER = 9470; // Set PWM time base period to 20ms (15-bit value)
    _PTEN = 1;    // Enable PWM time base to start generating pulses
}
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