Week 10: Fight Code

After getting the PWM and the range sensor working I moved onto the actual fight code, I ended up writing a few different codes with different strategy for each, some strategies were better than others like the first one I wrote would first turn left away from the centre and spin on the spot looking for the bot, if it wasn’t spotted the bot would drive across the ring turn around and repeat searching, if a bot was seen our bot would go full speed forward until the nothing was in range of the range sensor. The code is shown below:

//
// This code will make the bot first move to the side
// of the ring to get the other bot from the side if its in range.
// The then will search for the bot by spinning to the left then to
// the right, if nothing is seen the bot would move to the other side of
// the ring and repeat the searching
// Written By Ronan Byrne
// Last Updated 30/03/2015
//

#include <msp430.h>
#define REVERSE 0b00100010
#define FORWARD 0b00010001
#define STOP 0b00000000
#define S_LEFT 0b00010010
#define S_RIGHT 0b00100001
#define B_LEFT 0b00100000
#define B_RIGHT 0b00000010
#define State1 0b00000001
#define State2 0b00100000
#define State3 0b00100001

int main(void)
{
    WDTCTL = WDTPW + WDTHOLD; // Disable watchdog 
    
    P1DIR = 0b01100001; // P1.0 is an output, P1.1-7 are inputs
    P2DIR = 0b00110111; // P2.0,1,4,5 outputs, P2.2,3,6,7 inputs
    
    // Select primary peripheral module function on P2.2 (i.e. TA1.1)
    P2SEL  = 0b00000100;
    P2SEL2 = 0b00000000;
     
    // Configure Timer A interrupt FOR PWM
    TA1CTL = TASSEL_2 + ID_0 + MC_1; // Timer_A1: SMCLK clock, input divider=1, "up" mode
    TA1CCR0 = 20000;                  // Set Timer_A1 period to 20ms for 50Hz PWM
    TA1CCR1 = 10000;                   // 50% duty cycle initially (10s pulse width)
    TA1CCTL1 = OUTMOD_7;             // Select "Reset/Set" output mode
    
    // Make P1.0 a digital output for the LED and P1.1
    // a digital output for the sensor trigger pulses
    //P1DIR = 0b01000001; //FIX
     
    // Set up Timer_A1: SMCLK clock, input divider=1, FOR RANGEFINDER
    // "continuous" mode. It counts from 0 to 65535,
    // incrementing once per clock cycle (i.e. every 1us).
    TA1CTL = TASSEL_2 + ID_0 + MC_2;
    
    int d;         // distance
    int dc = 0;    // distance counter
    int state = 0; // state variable
    
    // Function prototypes
    unsigned int read_analog_channel(unsigned int);
    int distance();
    
    //int start = rand() % 2;
    //Random rnd = new Random();
    //int start = rnd.Next(0,2);
    
    /*if(start == 1)
    {
        P1OUT = 0b00100000;
    }
    else
    {
        P1OUT = 0b00000001;
    }*/
    
    while(1)
    {
        // Measure distance using rangfinder
        d = distance();
        
        // Define Colour Sensors
        int Front_S = read_analog_channel(3);
        int Back_S = read_analog_channel(2);
        
        // Check if sensor sees white
        if(Front_S > 512) state = 4;
        else if(Back_S > 512) state = 5; 
        
        // Update distance counter
        else if (d < 30) dc = 10;          // positive reading sets dc to 10
        else if (dc > 0) dc = dc - 1; // negative reading reduces dc by 1
        
        // Start by turning left drive forward then turn right, change to state 1
        else if(state == 0)
        {
            P2OUT = S_LEFT;
            __delay_cycles(50000);
            P2OUT = FORWARD;
            __delay_cycles(100000);
            P2OUT = S_RIGHT;
            __delay_cycles(50000);
            if(dc >= 5) state = 3;
            state = 1;
        }
        // Spin left then spin right looking for bot
        else if(state ==1)
        {
            P1OUT = State1;
            P2OUT = S_LEFT;
            __delay_cycles(10000);
            if(dc >= 5) state = 3;
            P2OUT = S_RIGHT;
            __delay_cycles(15000);
            if(dc >= 5) state = 3;
            state = 2; 
        }
        // Move across ring and turn around  
        else if(state == 2)
        {
            P1OUT = State2;
            P2OUT = FORWARD;
            __delay_cycles(200000);
            if(dc >= 5) state = 3;
            P2OUT = S_LEFT;
            __delay_cycles(100000);
            if(dc >= 5) state = 3;
            state = 1;  
        }
        // Attack other bot 
        else if(state == 3)
        {
            P1OUT = State3;
            TACCR1 = 20000;
            P2OUT = FORWARD;
            if(dc == 0)
            {
                TACCR1 = 10000;
                state = 1;
            }
        }
        // Reverse from white line and turn around
        else if(state == 4)
        {
            TACCR1 = 20000;
            P2OUT = REVERSE;
            __delay_cycles(50000);
            P2OUT = S_LEFT;
            __delay_cycles(100000);
            TACCR1 = 10000;
            state = 1;
        }
        // Drive forward from white line
        else if (state == 5)
        {
            TACCR1 = 20000;
            P2OUT = FORWARD;
            __delay_cycles(50000);
            TACCR1 = 10000;
            state = 1;
        }
    }
    return 0;
}

//
// This function measures distance in cm using the rangefinder.
// It assumes TRIG is P1.6 and ECHO is P1.7.
//
int distance()
{
    // Send 20us trigger pulse
    P1OUT |= BIT6;
    __delay_cycles(20);
    P1OUT &= ~BIT6;
      
    // Wait for start of echo pulse
    while((P1IN & BIT7) == 0);
      
    // Measure how long the pulse is
    int d = 0;
    while((P1IN & BIT7) > 0)
    {
        // The following delay was worked out by trial and error
        // so that d counts up in steps corresponding to 1cm
        __delay_cycles(30);
        d = d + 1;
        if (d >= 400) break;
    }
     
    // Leave 100ms for any ultrasound reflections to die away
    __delay_cycles(100000);
     
    // Pass the measured distance back to the calling function
    return d;
}

//
// This function performs a single analog to digital conversion,
// converting the voltage on analog input pin ANx into a 10-bit
// unsigned integer. Execution time for this function is of the
// order of 100us.
//
unsigned int read_analog_channel(unsigned int x)
{
    ADC10CTL0 &= ~ENC;            // disable conversion
    ADC10CTL1 = x << 12;          // select channel
    ADC10CTL0 |= ENC;             // enable conversion
    ADC10CTL0 |= ADC10SC;         // start conversion
    while(ADC10CTL1 & ADC10BUSY); // wait until complete
    return ADC10MEM;              // return digital value
}

This code was meant to also to have a random start for example it would move to the right of the ring at start up or left of the ring but I couldn’t get the random state code working. This wasn’t the main reason for not using this code, the main reason was that the bots would be placed at random angles at the start of the fight which wouldn’t work with this code, another problem was that I used delays in all the states which I would try to break out of with interrupts (not in the code) which I also couldn’t get working, because of the delays without interrupts the bot wouldn’t stop until it was finished whatever its doing this could drive the bot of the ring or not seeing the bot in search mode so this code wasn’t used. After I finished this code I didn’t know that the bot was put in a random angles, I found this out later, so I changed the code to get rid most of the delays, the way I did this was create a timer that increased every cycle of the code, so every cycle the bot would check its sensor then repeat until the timer count was reached unless one of the sensors changed the state, but this code was over written. For the final code I used the following state machine diagram to write the code:
State Machine Diagram
This code was the kamikaze code where the bot would just run around the ring relying more on the limit switch than the range sensor as depending on the range sensor was unreliable, the range sensor was used in this code but just not as much as previous code, the timer was also left out of this code.

// 
// This code is less reliant on the range sensor 
// and more on the chance of meeting the bot by 
// driving around the ring, the bot also reacts 
// to the outer ring with colour sensor and a 
// limit switch which is used to detect the other bot
// Written by Ronan Byrne- last updated 22/04/2015
//
  
#include <msp430.h>
#define REVERSE 0b00100010
#define FORWARD 0b00010001
#define STOP 0b00000000
#define S_LEFT 0b00010010
#define S_RIGHT 0b00100001
#define B_LEFT 0b00100000
#define B_RIGHT 0b00000010
#define State1 0b00000001
#define State2 0b00100000
#define State3 0b00100001

// function prototype
int distance();
unsigned int read_analog_channel(unsigned int);
 
int main()
{
    int d;  // distance
    int dc = 0;    // distance counter
    int State = 1; // state variable
    int sw = BIT1; // Limit Switch
    int FrontL_s,FrontR_s,Back_s; // Colour Sensors
    
    
    // Watchdog timer
    WDTCTL = WDTPW + WDTHOLD; // Disable watchdog timer
     
    // Select primary peripheral module function on P2.2 (i.e. TA1.1)
    P2SEL  = 0b00000100;
    P2SEL2 = 0b00000000;
    
    P2DIR  = 0b00110111; //Outputs P2.0 & P2.1 Left Motor, P2.2 PWM, P2.4 & P2.5
                         //Right Motors
    P1DIR = 0b01100000;  //Outputs P1.0 LED, P1.5 LED, P1.6 Trig. Inputs P1.2 SW
                         //P1.3 Front_s, P1.4 Back_s P1.7 Echo  
    
    // Analog inputs
    ADC10AE0 = 0b00001100; // A2,A3 are analog inputs
    ADC10CTL0 = ADC10ON;   // Turn on the ADC
     
    // Configure Timer A interrupt
    TA1CTL = TASSEL_2 + ID_0 + MC_1; // Timer_A1: SMCLK clock, input divider=1, "up" mode
    TA1CCR0= 20000;                  // Set Timer_A1 period to 20ms for 50Hz PWM
    TA1CCR1 = 10000;                 // 50% duty cycle initially (10ms pulse width)
    TA1CCTL1 = OUTMOD_7;             // Select "Reset/Set" output mode
    __delay_cycles(1000000)          // Two second delay (Takes around a second to configure above)
    while(1)
    {
        d=distance();
        FrontL_s = read_analog_channel(2);
        FrontR_s = read_analog_channel(0);
        Back_s = read_analog_channel(3);
        if (d < 30) dc = 10;  // If distance less than 30cm set counter to 10
        else if (dc > 0) dc = dc - 1; //If distance is greater than 0, minis one
        
        
        //Front Left sensor sees white, reverse then turn right unless switch is pressed
        if(State == 2) 
        {
            TA1CCR1 = 20000;
            while (P1IN & sw);//wait until a switch is not pressed
            P2OUT = REVERSE;
            __delay_cycles(500000);
            P2OUT = S_RIGHT;
            __delay_cycles(750000);
            State = 1;
        }
        //Front right sensor sees white, reverse then turn left unless switch is pressed 
        else if(State == 4)
        {
            TA1CCR1 = 20000;
            while (P1IN & sw);//wait until a switch is not pressed
            P2OUT = REVERSE;
            __delay_cycles(500000);
            P2OUT = S_LEFT;
            __delay_cycles(750000);
            State = 1;
        }
        //Range sensor sees bot or switch pressed, push until white line and not switch
        else if (State == 5)
        {
            TA1CCR1 = 20000;
            P2OUT = FORWARD;
            if(((FrontL_s >= 512) || (FrontR_s >= 512)) && ((P1IN & sw) < 1)) State = 2;
        }
        //Back Sensor sees white, drive forward
        else if(State == 3)
        {
            P2OUT = FORWARD;
            TA1CCR1 = 20000;
            if((Back_s < 512) && ((P1IN & sw) < 1))
            {
                State = 1;
            }
        }
        //Drive straight until white line or see bot
        else if (State == 1)
        {
            TA1CCR1 = 10000;
            P2OUT= FORWARD;
            if ((dc > 0) || ((P1IN & sw) > 1)) 
            {
                State = 5;
            }
            else if (FrontR_s >=512) State = 4;
            else if(FrontL_s >=512) State = 2;
            else if(Back_s >=512) State = 3;
        }
    }
     
    return 0;
}
//Calculate distance
int distance()
{
    // Send 20us trigger pulse
    P1OUT |= BIT6;
    __delay_cycles(20);
    P1OUT &= ~BIT6;
      
    // Wait for start of echo pulse
    while((P1IN & BIT7) == 0);
      
    // Measure how long the pulse is
    int d = 0;
    while((P1IN & BIT7) > 0)
    {
        // The following delay was worked out by trial and error
        // so that d counts up in steps corresponding to 1cm
        __delay_cycles(30);
        d = d + 1;
        if (d >= 400) break;
    }
     
    // Leave 100ms for any ultrasound reflections to die away
    __delay_cycles(100000);
     
    // Pass the measured distance back to the calling function
    return d;
}
//
// This function performs a single analog to digital conversion,
// converting the voltage on analog input pin ANx into a 10-bit
// unsigned integer. Execution time for this function is of the
// order of 100us.
//
unsigned int read_analog_channel(unsigned int x)
{
    ADC10CTL0 &= ~ENC;            // disable conversion
    ADC10CTL1 = x << 12;          // select channel
    ADC10CTL0 |= ENC;             // enable conversion
    ADC10CTL0 |= ADC10SC;         // start conversion
    while(ADC10CTL1 & ADC10BUSY); // wait until complete
    return ADC10MEM;              // return digital value
}

This code worked when tested on its own but things might be different when tested in the bot. Now that the code was out of the way it was time to actually build the bot which Dave took most of the responsibility for, by now the wheels and motors have arrived and Nathan had drawings for the bot sent off to be cut in the laser cutter. Everything would hopefully work together with no problems next week.

Advertisements

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