Introducing Robotics 3.1…

Robotics 3.1 is an optional module available to final-year students of the level 7 Electrical & Control Engineering programme (DT009) in the Dublin Institute of Technology. The learning outcomes of the module are:

  • Explain, carry out analysis and solve problems associated with fundamental electrical and mechanical engineering science,
  • List, outline and discuss comparative features of robot technology,
  • Explain and compare the operation, characteristics, and application of robot actuators,
  • Design, build and test electric robot actuator control subsystems,
  • Explain and compare the operation, application and characteristics of robot environment sensors,
  • Design, build and test robot environment sensing subsystems,
  • Design, build and test computer and microcontroller controlled robot subsystems.

The module is very practically focused and involves implementing various robot subsystems. Each student also undertakes a mini-project in which he or she designs, builds and tests a small robotic system.

The revision material presented here is divided into the following sections:

  1. Introduction to robotic technology
  2. Fundamental electrical, mechanical and control principles
  3. The dsPIC microcontroller
  4. Actuators and power electronics

To assist in exam preparation, the structure of each section combines revision notes with additional content in question and answer format.

Standard Hardware Kit for Robotics 3.1

  • 1 ziploc bag
  • 1 geared dc motor
  • 1 stepper motor
  • 1 servo motor
  • 1 breadboard
  • 1 PICkit2
  • 1 USB to mini USB cable
  • 1 dsPIC30F4011 microcontroller
  • 1 SN754410NE quad half H-bridge IC
  • 2 BC237 NPN transistors (or similar)
  • 1 6-pin header
  • A few LEDs
  • A few resistors
  • 1 potentiometers
  • 1 4xAA battery holder
Advertisements
Posted in Uncategorized | Leave a comment

HC-SR04 ultrasonic rangefinder example code for dsPIC30F4011

The following example program for a dsPIC30F40011 microcontroller takes a reading from a HC-SR04 ultrasonic rangefinder once every 200ms.

  • The dsPIC 30F4011 is running at 30 MIPS (using the internal fast RC oscillator with 16x PLL multiplier).
  • RC13 is a digital output and is connected to the “trigger” pin on the HC-SR04.
  • RC14 is a digital input and is connected to the “echo” pin on the HC-SR04.
  • The HC-SR04 requires trigger pulses of 10 us duration (or longer). In this example, the trigger pulses are 20 us in duration.
  • Timer 1 is used to measure the duration of the echo pulses. Its prescaler is set to 1:8, so that each timer tick is equal to 8 instruction cycles (266.66 ns).
  • The UART is configured at 38400 baud to print the measured distance (in metres).
//
// dsPIC30F4011 Ultrasonic rangefinder example
// Written by Ted Burke
// Last updated 25-10-2017
//
// RC13 is the trigger pin (digital output)
// RC14 is the echo pin (digital input)
//
  
#include <xc.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
  
void main()
{
    // Configure RC13 as a digital output to send the trigger pulse
    _TRISC13 = 0;
    
    // Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
  
    // We'll use Timer 1 to measure the duration of the echo pulse
    // We set the Timer 1 prescaler to 1:8, which means that each tick
    // of the timer is equal to 8 instruction cycles - i.e. 266.66 ns
    PR1 = 65535;          // Set the Timer 1 period (max 65535)
    T1CONbits.TCKPS = 1;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
    
    double distance;
    
    while(1)
    {
        // Send trigger pulse
        _LATC13 = 1;                        // begin trigger pulse
        __delay32(600);                     // 20 us
        _LATC13 = 0;                        // end trigger pulse
        
        // Measure duration of echo pulse
        while (_RC14 == 0);                 // wait for beginning of echo pulse
        TMR1 = 0;                           // reset Timer 1 counter
        T1CONbits.TON = 1;                  // start Timer 1
        while (_RC14 == 1 && TMR1 < 50000); // wait for end of echo pulse (timeout after 13.33 ms)
        T1CONbits.TON = 0;                  // stop Timer 1
        
        // Calculate the distance from duration of echo pulse
        distance = TMR1 * 4.5333e-5;
        printf("Distance: %f metres\r\n", distance);
        
        // Delay (200 ms) before next reading
        __delay32(6000000);
    }
}
Posted in Uncategorized | Leave a comment

Slaughterbots video

This thought-provoking video explores some of the dangers of so-called autonomous weapons. I think it’s important for all of our futures that more people become aware of the debate about autonomous weapons. Videos like this seem like a very effective way of making people understand the threat.

Warning: This video contains some violent scenes

Posted in Uncategorized | Leave a comment

Two alternative state machine structures with Timer 1 millisecond counter

These are the two versions of the dsPIC30F state machine code we looked at in today’s class, both of which use a Timer 1 interrupt service routine to increment a millisecond counter that is used to trigger timeout state transitions. The two versions are functionally more or less equivalent, but one may produce clearer code than the other in certain situations.

The first version is what I call “if inside while”. A single while(1) loop drives the state machine. Each time around the loop, all sensors are read at the start of the loop and then an if-else-if statement is used to select the code for the current state.

//
// dsPIC30F4011 state machine example with timer
// Version 1 - "if inside while"
//
// Written by Ted Burke, 13-11-2017
//

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

#define M_PI 3.141592653589793

// 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

// Function prototype for Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void);

// Millisecond counter and state variable
int millis = 0;
int state = 1;  

void main()
{
    // Make every pin in Port D an output
    TRISD = 0;

    // Configure Timer 1 to generate interrupt every 1ms
    PR1 = 30000;          // Set the Timer 1 period (max 65535)
    TMR1 = 0;             // Reset Timer 1 counter
    IEC0bits.T1IE = 1;    // Enable Timer 1 interrupt
    T1CONbits.TCKPS = 0;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
    T1CONbits.TON = 1;    // Turn on Timer 1
    
    // State loop (if inside while)
    while(1)
    {
        // Read sensors
        //colour = analogRead(0);
        //distance = analogRead(1);
        
        if (state == 1)
        {
            // Set actuators
            LATD = 0b0001;
            
            // Change state?
            if (millis > 500) gotoState(2);
        }
        else if (state == 2)
        {
            // Set actuators
            LATD = 0b0010;
            
            // Change state?
            if (millis > 1000) gotoState(3);
        }
        else if (state == 3)
        {
            // Set actuators
            LATD = 0b0100;
            
            // Change state?
            if (millis > 1500) gotoState(1);
        }
    }
}

// Reset millisecond counter and change state
void gotoState(int newState)
{
    millis = 0;
    state = newState;
}

// Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
    // Clear Timer 1 interrupt flag
    IFS0bits.T1IF = 0;
  
    millis = millis + 1;
}

The second version is what I call “whiles inside while”. In this structure, each state has its own dedicated while loop, which must include reading any relevant sensors as well as setting actuators and checking for state transitions. Each state’s loop can be preceded by initialisation code if required (e.g. resetting the millisecond timer). If initialisation code should only be executed when actually entering the state, then it can be enclosed in an if statement.

//
// dsPIC30F4011 state machine example with timer
// Version 2 - "whiles inside while"
//
// Written by Ted Burke, 13-11-2017
//

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

#define M_PI 3.141592653589793

// 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

// Function prototype for Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void);

// Millisecond counter and state variable
int millis = 0;
int state = 1;  

void main()
{
    // Make every pin in Port D an output
    TRISD = 0;

    // Configure Timer 1
    PR1 = 30000;          // Set the Timer 1 period (max 65535)
    TMR1 = 0;             // Reset Timer 1 counter
    IEC0bits.T1IE = 1;    // Enable Timer 1 interrupt
    T1CONbits.TCKPS = 0;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
    T1CONbits.TON = 1;    // Turn on Timer 1
    
    // State loop (whiles inside while)
    while(1)
    {
        millis = 0;
        while (state == 1)
        {
            // Set actuators
            LATD = 0b0001;
            
            // Read relevant sensors
            // ?????
            
            // Change state?
            if (millis > 500) state = 2;
        }
        
        millis = 0;
        while (state == 2)
        {
            // Set actuators
            LATD = 0b0010;
            
            // Read relevant sensors
            // ?????
            
            // Change state?
            if (millis > 1000) state = 3;
        }
        
        millis = 0;
        while (state == 3)
        {
            // Set actuators
            LATD = 0b0100;
            
            // Read relevant sensors
            // ?????
            
            // Change state?
            if (millis > 1500) state = 1;
        }
    }
}

// Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
    // Clear Timer 1 interrupt flag
    IFS0bits.T1IF = 0;
  
    millis = millis + 1;
}
Posted in Uncategorized | Leave a comment

A handy “super example” for the dsPIC30F4011

This is a handy “super example” for the dsPIC30F4011, which uses several features of the microcontroller:

  • Sets all Port D pins as outputs (I have LEDs connected to all four).
  • Toggles an LED on RD0 every 200ms approx.
  • Configures AN0-AN8 as analog inputs.
  • Enables all three PWM channels in complimentary mode, with period set to 20 ms (suitable for servo control).
  • Configures the UART for printing analog voltage read on AN0
  • Characters are also read via UART to control some of the outputs: “2” toggles RD2, “3” toggles RD3, “s” steps the servo angle on PWM channel 1.
  • Configures the Timer 1 interrupt to call an interrupt service routine every 125ms.

When using this as template code for a new project, just delete the featured you don’t need and edit the ones you do.

//
// dsPIC30F4011 Super Example
// Written by Ted Burke
// Last updated 19-10-2017
//
// Featuring digital i/o, analog input,
// PWM output, UART Tx/Rx, Timer 1 ISR.
//
 
#include <xc.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 prototype for analog read function
unsigned int analogRead(int);
 
// Function prototype for Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void);
 
int main()
{
    double pw;   // Pulse width for servo (min 1ms, max 2ms)
    int voltage; // Analog voltage read which will be read from AN0
    char c;      // The char data type is basically just an 8-bit int
 
    ///////////////////////////////////////////////////////////////
    // Set up digital i/o, analog input, PWM, UART and interrupt //
    ///////////////////////////////////////////////////////////////
    
    // Configure all 4 port D pins as digital outputs
    TRISD = 0;
 
    // 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 = 937;           // 1.0ms pulse width on PWM channel 1
    PDC2 = 937;           // 1.0ms pulse width on PWM channel 2
    PDC3 = 937;           // 1.0ms 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
 
    // Configure Timer 1
    // In this example, I'm setting PR1 and TCKPS for 8Hz
    PR1 = 14648;          // Set the Timer 1 period (max 65535)
    TMR1 = 0;             // Reset Timer 1 counter
    IEC0bits.T1IE = 1;    // Enable Timer 1 interrupt
    T1CONbits.TCKPS = 3;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
    T1CONbits.TON = 1;    // Turn on Timer 1

    ///////////////////////
    // Main control loop //
    ///////////////////////
    
    while(1)
    {
        // Read a voltage from AN0 and print it via serial port
        voltage = analogRead(0);
        printf("Voltage = %d\r\n", voltage);
 
        // Toggle LED on RD0
        _LATD0 = 1 - _LATD0;
 
        // Check if any characters were received via UART
        if (U1STAbits.URXDA == 1)
        {
            c = U1RXREG;                       // read the character
            if (c == '2') _LATD2 = 1 - _LATD2; // toggle RD2 on/off
            if (c == '3') _LATD3 = 1 - _LATD3; // toggle RD3 on/off
            if (c == 's') pw = pw + 0.2;       // increase the servo angle
        }
        
        // Set servo angle by setting pulse width on PWM channel 1
        if (pw > 2.0) pw = 1.0;
        PDC1 = pw * 937.5; // Note: 937.5 units is equivalent to 1ms
 
        __delay32(6000000); // 200ms delay
    }
 
    return 0;
}
 
// Timer 1 interrupt service routine
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
    // Clear Timer 1 interrupt flag
    IFS0bits.T1IF = 0;
 
    // Toggle LED on RD1
    _LATD1 = 1 - _LATD1;
}
  
// 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 analogRead(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 is the build script I’m using:

xc16-gcc main.c -mcpu=30F4011 -Wl,--script=p30F4011.gld
if errorlevel 0 xc16-bin2hex a.out
Posted in Uncategorized | 1 Comment

Millisecond timer for state machine using Timer 1 interrupt

//
// Timer interrupt example for dsPIC30F4011
// Written by Ted Burke - 16-11-2016
//
// This is a simple state machine with four
// states. The current state is indicated
// using two LEDs. All state transitions are
// triggered by time elapsed since the
// previous state transition, as measured
// using a millisecond counter which is
// incremented every 30000 instruction cycles
// by a Timer 1 interrupt service routine.
//

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

_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Clock speed = 7.5MHz x 16, i.e. 30 MIPS
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

// These variables must be global because they
// are accessed by the main function and the ISR
int state = 1;        // state variable
int milliseconds = 0; // millisecond counter

// Timer 1 interrupt service routine (ISR)
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
    _T1IF = 0;      // Clear Timer 1 interrupt flag
    milliseconds++; // Increment the millisecond counter
}

// This function resets the millisecond counter and changes state
void change_state(int n)
{
    milliseconds = 0;
    state = n;
}

int main()
{
    // Configure digital I/O
    TRISD = 0b1100; // RD0 and RD1 are the two LEDs
    
    // Configure Timer 1
    PR1 = 30000;          // Set the Timer 1 period to 1 ms (30000 * Tcy)
    TMR1 = 0;             // Reset Timer 1 counter
    _T1IE = 1;            // Enable Timer 1 interrupt
    T1CONbits.TCKPS = 0;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
    T1CONbits.TON = 1;    // Turn on Timer 1
    
    while(1)
    {
        if (state == 1)
        {
            // Display state on LEDs
            _LATD0 = 0;
            _LATD1 = 0;
            
            // Change state?
            if (milliseconds >= 500) change_state(2);
        }
        else if (state == 2)
        {
            // Display state on LEDs
            _LATD0 = 1;
            _LATD1 = 0;
            
            // Change state?
            if (milliseconds >= 500) change_state(3);
        }
        else if (state == 3)
        {
            // Display state on LEDs
            _LATD0 = 0;
            _LATD1 = 1;
            
            // Change state?
            if (milliseconds >= 2000) change_state(4);
        }
        else if (state == 4)
        {
            // Display state on LEDs
            _LATD0 = 1;
            _LATD1 = 1;
            
            // Change state?
            if (milliseconds >= 3000) change_state(1);
        }
    }
        
    return 0;
}
Posted in Uncategorized | Leave a comment

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;
}
Posted in Uncategorized | Leave a comment

Stepper motor reference position example from today’s class

//
// Stepper control example for dsPIC30F4011
// Written by Ted Burke
// Last updated 13-10-2016
//

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

_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Clock speed = 7.5MHz x 16, i.e. 30 MIPS
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

#define STEP_DELAY 150000

// Function prototype for step functions and analog read
void step_forward();
void step_back();
void steps(int count);
unsigned int read_analog_channel(int channel);

int main()
{
    // Configure digital I/O
    TRISD = 0b0000; // One digital output for each winding
    LATD  = 0b0000; // Initially, all windings are off
    
    // Setup UART
    U1BRG = 48;            // 38400 baud @ 30 MIPS
    U1MODEbits.UARTEN = 1; // Enable UART
 
    // Configure AN0-AN8 as analog inputs
    ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
    ADCON1bits.ADON = 1;   // Turn ADC ON
 
    // Move to reference position by stepping until the analog
    // voltage from the colour sensor exceeds a threshold
    while(read_analog_channel(0) < 512) step_forward();
    
    // Stay still for 2 seconds
    __delay32(60000000);
    
    // Then start ticking once every second (approx)
    while(1)
    {
        steps(34);
        __delay32(30000000);
    }
    
    return 0;
}

// This function moves the stepper motor through
// the steps specific in the "count" argument.
// Positive values result in forward steps.
// Negative values result in backwards steps.
void steps(int count)
{
    while (count > 0)
    {
        step_forward();
        count--;
    }
    
    while (count < 0)
    {
        step_back();
        count++;
    }
}

void step_forward()
{
    if      (LATD == 0b0001) LATD = 0b0010;
    else if (LATD == 0b0010) LATD = 0b0100;
    else if (LATD == 0b0100) LATD = 0b1000;
    else                     LATD = 0b0001;
    
    __delay32(STEP_DELAY);
}

void step_back()
{
    if      (LATD == 0b0001) LATD = 0b1000;
    else if (LATD == 0b0010) LATD = 0b0001;
    else if (LATD == 0b0100) LATD = 0b0010;
    else                     LATD = 0b0100;
    
    __delay32(STEP_DELAY);
}

// 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;
}
Posted in Uncategorized | Leave a comment