Two SCARA control examples from today’s class

This first example moves the SCARA’s end effector to the x,y point defined by the analog input voltages present on pins AN0 and AN1 respectively.

//
// dsPIC30F4011 SCARA Example
// In this example the x and y coordinates of the SCARA's end effector 
// are controlled by the analog voltages on AN0 and AN1 respectively.
// Written by Ted Burke
// Last updated 20-10-2016
//
 
#include <xc.h>
#include <math.h>
#include <stdio.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

// Function prototypes
unsigned int read_analog_channel(int n);

// Segment lengths in cm
const double L1 = 20.0;
const double L2 = 15.0;

int main()
{
    int Vx, Vy; // analog input voltages
    double x, y; // end effector position in cm
    
    // Configure AN0-AN8 as analog inputs
    ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
    ADCON1bits.ADON = 1;   // Turn ADC ON
 
    // Configure PWM for free running mode
    //
    //   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
    //   PWM pulse width = (Tcy/2) * prescale * PDCx
    //
    PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
    PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
    PTPER = 9375;         // 20ms PWM period (15-bit period value)
    PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
    PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
    PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
    PTMR = 0;             // Clear 15-bit PWM timer counter
    PTCONbits.PTEN = 1;   // Enable PWM time base
 
    // Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
 
    while(1)
    {
        // Read target position from potentiometers
        Vx = read_analog_channel(0); // 0-1023 represents 10 to 20 cm
        Vy = read_analog_channel(1); // 0-1023 represents 10 to 20 cm
        
        // convert into x,y coordinates in cm
        x = 10.0 + (10.0 * Vx / 1023.0);
        y = 10.0 + (10.0 * Vy / 1023.0);
        
        // Set SCARA position
        setSCARA(x, y);     
    }
 
    return 0;
}

//
// This function moves the end effector to the specified (x,y) point
//
void setSCARA(double x, double y)
{
    double S, E;
    
    // Inverse kinematique 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)));
    
    // 937.5 PDC units per ms
    // 1ms < pw < 2ms -> 0 < S < 180
    // 1.0ms : 0deg, 1.5ms : 90deg, 2.0ms : 180deg 
    PDC1 = 937.5 * (1.0 + 1.0*S/M_PI);
    PDC2 = 937.5 * (1.0 + 1.0*E/M_PI);
}

//
// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
//
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // Start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // Start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 3.2us
    return ADCBUF0;
}

This second example moves the SCARA’s end effector through a repeating sequence of four pre-defined x,y coordinates, stopping for 1 second at each point.

//
// dsPIC30F4011 SCARA Example
// This example steps through four pre-defined (x,y) points repeatedly.
// Written by Ted Burke
// Last updated 20-10-2016
//
 
#include <xc.h>
#include <math.h>
#include <stdio.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

// Function prototypes
unsigned int read_analog_channel(int n);

// Segment lengths cm
const double L1 = 20.0;
const double L2 = 15.0;

int main()
{
    int Vx, Vy; // analog input voltages
    double x, y; // end effector position in cm
    
    // Configure AN0-AN8 as analog inputs
    ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
    ADCON1bits.ADON = 1;   // Turn ADC ON
 
    // Configure PWM for free running mode
    //
    //   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
    //   PWM pulse width = (Tcy/2) * prescale * PDCx
    //
    PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
    PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
    PTPER = 9375;         // 20ms PWM period (15-bit period value)
    PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
    PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
    PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
    PTMR = 0;             // Clear 15-bit PWM timer counter
    PTCONbits.PTEN = 1;   // Enable PWM time base
 
    // Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
 
    double x[] = {10.0, 12.0, 13.5, 10.0};
    double y[] = {10.0, 18.0, 20.0, 15.0};
    
    // This is the main loop that steps through the points
    int n = 0;
    while(1)
    {
        setSCARA(x[n], y[n]);
        n = n + 1;
        if (n == 4) n = 0;
        __delay32(30000000); // 1 second delay at each point
    }
 
    return 0;
}

//
// This function moves the end effector to the specified (x,y) point
//
void setSCARA(double x, double y)
{
    double S, E;
    
    // Inverse kinematique 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)));
    
    // 937.5 PDC units per ms
    // 1ms < pw < 2ms -> 0 < S < 180
    // 1.0ms : 0deg, 1.5ms : 90deg, 2.0ms : 180deg 
    PDC1 = 937.5 * (1.0 + 1.0*S/M_PI);
    PDC2 = 937.5 * (1.0 + 1.0*E/M_PI);
}

//
// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
//
unsigned int read_analog_channel(int channel)
{
    ADCHS = channel;          // Select the requested channel
    ADCON1bits.SAMP = 1;      // Start sampling
    __delay32(30);            // 1us delay @ 30 MIPS
    ADCON1bits.SAMP = 0;      // Start Converting
    while (!ADCON1bits.DONE); // Should take 12 * Tad = 3.2us
    return ADCBUF0;
}
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