Body Guard Bot (Build)

For my mini-project, I have decided to create a simple bodyguard robot. Its main functions are protect something in a certain place area and if someone were to come close to that area the robot will try to prevent you from entering that area.

All my previous codes were built and designed to work with the final mini-project.

The construction of the robot uses two pan and tilt servo motor kits which were purchased from the robotshot.com and a few other parts to keep the whole system together.

Movement of the arms (servo motors) are controlled by the output compare [OC1 – OC4] module within the DSPIC main reason why I swapped from the PWM channels to the output compare was I had 4 servo motors to be used and only 3 PWM channels, also I had two motors that needed to be used for PWM to vary the speed at which they turn. So the best and efficent way of controlling the servo motors was with the output compare module located on all 4 RD pins, the motors, and the LED are controlled by RB pins configured in digital output mode [RB5 – RB9], while the IR range sensors the ADC [AN1 – AN2].

This slideshow requires JavaScript.

Circuit Diagram

servo-motor_schem

Although the circuit diagram above shows that there is an IR colour sensor, in the end, I wasn’t able to add it to follow or detect white lines on the ground, this was due to time constraints however in the future I plan to continue to upgrade this robot with more features.

Power Issue

After the robot was constructed both mechanically and electrically one noticeable problem after uploading the code onto the DSpic was that the robot was restarting very frequently. After setting up an oscilloscope and applying it across the voltage rail, it was noted that voltage was not stable due to power consumption from all the motors (a total of 6). So quick fix to allow for the voltage not to drop below 5volts and resetting the DSpic was to add another battery pack, in parallel (so that voltage doesn’t double but the current). With more current carrying capacity from the voltage source, the voltage rail was stable and the robot functioned without resetting.

img_3793

 

 

 

 

 

Possible States

In this code, I did not use a state machine as I felt there wasn’t a need to however, there are 4 possible actions coming from the bodyguard robot I have listed them down below.

 State  Action  Actuators State Transition
 1 Move Forward

Arms down

Left & Right Motor Forward

Left & Right Arms Down

 If Right & Left Range Finder Are within Threshold move to state 2

If Right Range Finder is within Threshold move to state 3

If Left Range Finder is within Threshold move to state 4

 2 Full Stop

Both Arms Up

Left & Right Motor Fully Stop

Left & Right Arms up

If Right Range Finder is within Threshold move to state 3

If Left Range Finder is within Threshold move to state 4

If Left & Right Range Finder are not within Threshold move to state 1

3 Turn Right

Right Arm Up

Right Motor Forward Left Motor Stop

Right Arm up       Left Arm Down

  If Right & Left Range Finder Are within Threshold move to state 2

If Left Range Finder is within Threshold move to state 4

If Left & Right Range Finder are not within Threshold move to state 1

 4  Turn Left

Left Arm Up

Left Motor Forward Right Motor Stop

Left Arm up             Right Arm Down

If Right & Left Range Finder Are within Threshold move to state 2

If Right Range Finder is within Threshold move to state 3

If Left & Right Range Finder are not within Threshold move to state 1

Code that runs this contraption


// Final Code that makes the body move forward, turn left or right
// depending on what the two range finder sensors see individually
// last updated 11-1-2017
// Written by Kevin Tighe
// code based on Batchloaf/Roboted.wordpress.com by Ted Burke

//important stuff
#include <xc.h>
#include <libpic30.h>

//Hash_define
#define Second 30000000

//More important stuff
_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

//Functions
unsigned int read_analog_channel(int channel);
void SetArmPositionLeft();
void SetArmPositionRight();
void Backward();
void Left();
void Right();
void Stop();

int main(void)
{
     // Configure RB pins to be digital and Analogue
    ADPCFG = 0b11111000;

    // Configure AN0as analog inputs
    ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
    ADCON1bits.ADON = 1;   // Turn ADC ON

    // Make RB4 and RD8 digital outputs;
    LATB = 0;
    TRISB = 0b000001111;

    // Configure Timer 2 (default timer for output compare)
    T2CONbits.TCKPS = 0b10; // Timer 2 prescaler 1:64
    PR2 = 9375;             // Timer 2 period (20ms)
    T2CONbits.TON = 1;      // Enable Timer 2

    // Configure Output Compare channels
    OC1CONbits.OCM = 0b101; // continuous pulse mode
    OC1R = 0;               // pulse start time
    OC1RS = 703;            // pulse stop time
    OC2CONbits.OCM = 0b101;
    OC2R = 0;
    OC2RS = 703;
    OC3CONbits.OCM = 0b101;
    OC3R = 0;
    OC3RS = 703;
    OC4CONbits.OCM = 0b101;
    OC4R = 0;
    OC4RS = 703; 

    // Configure PWM
    // PWM period = PTPER * prescale * Tcy = 9470 * 64 * 33.33ns = 20ms
    _PMOD1 = 0; // PWM channel 3 mode: 0 for complementary, 1 for independent
    _PEN1H = 1; // PWM1H 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)
    PDC1 = 0; // 0% duty cycle on channel 1 (16-bit value)
    _PTEN = 1; // Enable PWM time base to start generating pulses

    int r;
    for(r=0 ; r<10 ; ++r)
    {
        _LATB4 = 1;
        __delay32(1500000);
        _LATB4 = 0;
        __delay32(1500000);
    }

    // Configure max and min OC1RS values
    int min = 260;  // OC1RS value for 0 degrees
    int max = 1070; // OC1RS value for 180 degrees

    // An Array of angles for arm movement.
    //                            0      1      2      3      4      5      6      7      8     9     10     11     12     13     14     15     16     17     18
    float LeftShoulder  [] = {  0.0,  10.0,  20.0,  30.0,  40.0,  50.0,  60.0,  70.0,  80.0, 90.0, 100.0, 110.0, 120.0, 130.0, 140.0, 150.0, 160.0, 170.0, 180.0};
    float LeftElbow     [] = {  0.0,  10.0,  20.0,  30.0,  40.0,  50.0,  60.0,  70.0,  80.0, 90.0, 100.0, 110.0, 120.0, 130.0, 140.0, 150.0, 160.0, 170.0, 180.0};
    float RightShoulder [] = {180.0, 170.0, 160.0, 150.0, 140.0, 130.0, 120.0, 110.0, 100.0, 90.0,  80.0,  70.0,  60.0,  50.0,  40.0,  30.0,  20.0,  10.0,   0.0};
    float RightElbow    [] = {180.0, 170.0, 160.0, 150.0, 140.0, 130.0, 120.0, 110.0, 100.0, 90.0,  80.0,  70.0,  60.0,  50.0,  40.0,  30.0,  20.0,  10.0,   0.0};

    int threshold  = 256;
    //int threshold2 = 256

    while(1)
    {
        if      (read_analog_channel(1) < threshold && read_analog_channel(2) < threshold)
        {
        Right();
        Left();
        SetArmPositionRight(RightShoulder[0], RightElbow[7]);
        SetArmPositionLeft(LeftShoulder[0], LeftElbow[7]);
        __delay32(Second/10);
        }
        else if (read_analog_channel(1) > threshold && read_analog_channel(2) > threshold)
        {
            _LATB4 = 1;
            SetArmPositionRight(RightShoulder[12], RightElbow[10]);     //Right Arm up
            SetArmPositionLeft(LeftShoulder[12], LeftElbow[10]);        //Left Arm up
            Stop();                                                     //Stop Both Motors
            __delay32(Second/10);                                       //Delay to keep servo motors steady in position
        }

        else if (read_analog_channel(1) > threshold)                    //Right Sensor has detected something
        {
            SetArmPositionRight(RightShoulder[12], RightElbow[7]);      //Raise Right arm
            SetArmPositionLeft(LeftShoulder[0], LeftElbow[7]);
            Stop();
            Right();                                                    //Turn rightmotor to follow person
            __delay32(Second/10);
        }

        else if (read_analog_channel(2) > threshold)                    //Left Sensor has detected something
        {
            SetArmPositionRight(RightShoulder[0], RightElbow[7]);
            SetArmPositionLeft(LeftShoulder[12], LeftElbow[7]);         //Riase Left arm
            Stop();
            Left();                                                     //Turn leftmotor to follow person
            __delay32(Second/10);
        }
    }
}

//Arm Functions
void SetArmPositionLeft(double shoulder, double elbow)
{
    // Configure max and min OC1RS values
    int min = 260;  // OC1RS value for 0 degrees
    int max = 1070; // OC1RS value for 180 degrees

    OC1RS = min + (shoulder/180.0)*(max-min); // Left Arm
    OC4RS = min + (elbow/180.0)*(max-min);
}
void SetArmPositionRight(double shoulder, double elbow)
{
     // Configure max and min OC1RS values
    int min = 260;  // OC1RS value for 0 degrees
    int max = 1070; // OC1RS value for 180 degrees

    OC2RS = min + (shoulder/180.0)*(max-min);// Right Arm
    OC3RS = min + (elbow/180)*(max-min);
}

//Motor Functions
void Stop()
{
    _LATB5 = 0;
    _LATB6 = 0;
    _LATB7 = 0;
    _LATB8 = 0;
}

void Right()
{
    _LATB5 = 1;
    _LATB6 = 0;
    PDC1 = 0.9 * 2 * PTPER;     // 90% duty cycle
}

void Left()
{
    _LATB7 = 1;
    _LATB8 = 0;
    PDC1 = 0.9 * 2 * PTPER;     // 90% duty cycle
}

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

A selection of videos of the robot operational to the build Enjoy!

 

Advertisements

One thought on “Body Guard Bot (Build)

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 )

Twitter picture

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

Facebook photo

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

Google+ photo

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

Connecting to %s