## 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
}
```
This entry was posted in Uncategorized. Bookmark the permalink.