Automated Object Retrieval w Drone

#include <msp430.h>

#define LED1 BIT0                     // define LED1 bit
#define Motor1 BIT2                   //define + motor bit
#define Motor2 BIT6                   //define - motor bit
#define Echo1 BIT7
#define Trigger1 BIT4
#define Trigger2 BIT3
#define Echo2 BIT4
#define Echo1Low (!(Echo1 & P1IN))   ////1 and 2 Echo low/high conditions
#define Echo1High (Echo1 & P1IN)
#define Echo2Low (!(Echo2 & P2IN))
#define Echo2High (Echo2 & P2IN)
#define Motor1Low (!(Motor1 & P1IN))  //+ and - Motor low/high conditions
#define Motor1High (Motor1 & P1IN)
#define Motor2Low (!(Motor2 & P1IN))
#define Motor2High (Motor2 & P1IN)
#define TX BIT5                 // define Transmitter bit
#define TXLow (!(TX & P2IN))   ////Transceiver low/high conditions
#define TXHigh (TX & P2IN)
volatile unsigned int Time = 2000, Time2; //Timer dependent variable
unsigned const int Pulse1High = 100, Pulse1Low = 300; //PWM for + motor
char debounceTHigh();              //debounce TX high function



int main(void)
{
    ADC10CTL1 = INCH_5;                       // A5 single sequence
    ADC10CTL0 = ADC10SHT_2 + ADC10ON;         ///initialize ADC
    ADC10AE0 |= BIT5;                         // P1.5 ADC10 option select
    ADC10CTL0 &= ~ENC;

    P1IE |= Echo1;              //set ISR to echo
    P1IES &= ~Echo1;             //set low to high enable
    P1IFG &= ~Echo1;            // clear flag on echo

    P2IE |= Echo2;              //set ISR to echo2
    P2IES &= ~Echo2;             //set low to high enable
    P2IFG &= ~Echo2;            // clear flag on echo2

    WDTCTL = WDTPW | WDTHOLD;    //stop watchdog timer
    P1DIR = LED1 | Motor1 | Motor2 | Trigger1;        //set up motor, trigger, and LED for output 1
    P2DIR = Trigger2;                                 //set up trigger for output 2
    P1OUT = 0;                                 //initialize outputs low
    P2OUT = 0;

    __enable_interrupt();


      while(1)                                //repeat cycle indefinitely
         {
        if (debounceTHigh())                  //when transmitter pressed
         {
            __delay_cycles(2000000);          //wait 2 seconds
            P1OUT &= ~Motor2;                 //- motor is 0V
            P1OUT &= ~Trigger1;
            while(Time>=1500)                 //while floor is far away
               {
                 if(Motor1High)
                 __delay_cycles(Pulse1High);
                 else
                  __delay_cycles(Pulse1Low);
                 P1OUT ^= Motor1;             //toggle motor

                 P1OUT |= Trigger1;          //send trigger signal
                 __delay_cycles(10);
                 P1OUT &= ~Trigger1;

                while(Echo1Low)               // waiting for echo to rise
                   {
                    if(Motor1High)
                    __delay_cycles(Pulse1High);
                    else
                     __delay_cycles(Pulse1Low);
                    P1OUT ^= Motor1;             //toggle motor
                   }

                if(Echo1High)              //after echo ISR, comes here while echo high
                   {
                    while(Echo1High)
                      {
                        if(Motor1High)
                        __delay_cycles(Pulse1High);
                        else
                         __delay_cycles(Pulse1Low);
                        P1OUT ^= Motor1;            //toggle motor
                      }
                   }
                 Time = TA0R;                  //echo back low, record time

                 TA0CTL = MC_0 | TACLR;        // clear/stop timer
                 P1OUT |= Motor1;          //toggle motor on

                    __delay_cycles(40000);   //time in between measurements

                }
            P1OUT &= ~Motor1;              //+ motor off

            __delay_cycles(2000000);      //wait 2 sec

             while(ADC10MEM<400)          //measure force until object placed
              {
                ADC10CTL0 |= ENC + ADC10SC;             // Sampling and conversion start
                while (ADC10CTL1 & ADC10BUSY);          // ADC10BUSY?
              }
             __delay_cycles(2000000);     //wait 2 sec

             ADC10MEM = 0;                //reset ADC
             Time=2000;                   //reset Time variables
             Time2=2100;
             P2OUT &= ~Trigger2;
             while(Time2>1800)  //while drone is far away
                {

                   P2OUT |= Trigger2;     //send trigger2 signal
                   __delay_cycles(10);
                    P2OUT &= ~Trigger2;

                   while(Echo2Low);       // waiting for echo22 to rise

                    if(Echo2High)
                      {
                       while(Echo2High);   //after echo2 ISR, comes here while echo high
                      }
                   Time2 = TA0R;            //echo back low, record time

                    TA0CTL = MC_0 | TACLR;    // clear/stop timer
                    P1OUT |= Motor2;          //toggle - motor on, stays on
                     __delay_cycles(2000);    //time between measurements
                    }
                 P1OUT &= ~Motor2;           //- motor off
           }

       }
}

char debounceTHigh()
{
    __delay_cycles(100000);   //debounce time

    if (TXHigh)     //if TX pressed return true
        return 0x01;
    else
        return 0x00;    //otherwise return false
}

#pragma vector=PORT1_VECTOR       //echo1 ISR
__interrupt void ECHO1 (void)
{
    P1IFG &= ~Echo1;             //clear echo1 flag
    TA0CTL = TASSEL_2 | MC_2;     // SMCLK, up mode
}

#pragma vector=PORT2_VECTOR       //echo2 ISR
__interrupt void ECHO2 (void)
{
    P2IFG &= ~Echo2;             //clear echo2 flag
    TA0CTL = TASSEL_2 | MC_2;     // SMCLK, up mode
}