File content as of revision 0:6d4adda58ec2:

#include "mbed.h"
#include "uLCD_4DGL.h"
#include "Motor.h"
#include "rtos.h"
#include "stdio.h"
 
//Motors
Motor m_R(p21, p5, p6); // pwm, fwd, rev _ right wheel
Motor m_L(p22, p7, p8); // pwm, fwd, rev _ left wheel

//Serial communication between 2 mbeds
Serial pc(USBTX, USBRX);
Serial uart(p9, p10);

// Timer and halleffect sensor interrupts
Timer t;
InterruptIn risingEdge_L(p24);
InterruptIn risingEdge_R(p25);

// uLCD for testing
uLCD_4DGL uLCD(p28,p27,p29); 
 
// LEDs for testing
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

//global variables for the halleffect sensors 
volatile long int count_L;
volatile long int count_nr_L; 

volatile long int count_R;
volatile long int count_nr_R; 

volatile int count;
 
volatile char ms[1]; 
volatile char i ;

/*
volatile int speed_h = 6;
volatile int speed_m = 5;
volatile int speed_l = 4;
*/


void pulses_L() 
{
    if(myled2 == 1) 
    {
        myled2 = 0;
    } 
    else 
    {
        myled2 = 1;
    }
    count_L++;
    count_nr_L++;
}

void pulses_R() 
{
    if(myled4 == 1) 
    {
        myled4 = 0;
    } 
    else 
    {
        myled4 = 1;
    }
    count_R++;
    count_nr_R++;
}
 
int main() {
    uart.baud(9600);
    
    double circumference = 6.5 * 3.1416; // 6 cm wheel diameter * pi 
    long int temp_L;
    long int temp_R;
    double sA = 0.52;
    double sB = 0.55;

    double dist_L;
    double dist_R;
                
    count = 0;
    
    //risingEdge.mode(Pullup);
    risingEdge_L.rise(&pulses_L); 
    risingEdge_L.fall(&pulses_L);
    
    risingEdge_R.rise(&pulses_R);
    risingEdge_R.fall(&pulses_R);
    
    while(1) {
        
        myled2 = 1;
        count++;
        uLCD.locate(0,1);
        uLCD.printf("it's Ready, %d",count);
        
        if(uart.readable()) 
        {
            uLCD.locate(5,2);
            i = uart.getc();
            uLCD.printf("%c\n", i);  
                        
/////////////////////////////////////// Go forward ////////////////////////////
            if(i == 'f')
            {
                //speedA = speed_h;
                
                uLCD.locate(0,3);
                uLCD.printf("Go Forward!");
                
                m_L.speed(sA);
                m_R.speed(sB); 
                
                count_L = 0;
                count_R = 0;
                
                while(!(uart.readable()))
                {
                    temp_L = count_L;
                    temp_R = count_R;
                    
                    uLCD.locate(0,4);
                    uLCD.printf("Count_L: %d", temp_L);
            
                    uLCD.locate(0,5);
                    uLCD.printf("Count_R: %d", temp_R);
            
                    dist_L = (circumference/8)*((double)temp_L);
                    dist_R = (circumference/8)*((double)temp_R);
                    
                    uLCD.locate(0,7);
                    uLCD.printf("distance_L: %0.2f cm", dist_L);
                    uLCD.locate(0,8);
                    uLCD.printf("distance_R: %0.2f cm", dist_R);
                }
                
                
                
            }
/////////////////////////////////////// Go Backward  ////////////////////////////            
            if(i == 'b')
            {
                //speedA = speed_h;
                
               // uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Go Backward!");
                m_L.speed(-sA);
                m_R.speed(-sB); 
                
            }
/////////////////////////////////////// Turn left ////////////////////////////            
            if(i == 'l')
            {
               // speedA = speed_m;
                
                uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Turn Left!");
                m_L.speed(-0.65);
                m_R.speed(0.67); 
                
                
                count_L = 0;
                count_R = 0;
                
                while(!(uart.readable()))
                {
                    if((count_L < 4) || (count_R <4))
                    {
                        /*
                        temp_L = count_L;
                        temp_R = count_R;
                        
                        uLCD.locate(0,4);
                        uLCD.printf("Count_L: %d", temp_L);
                
                        uLCD.locate(0,5);
                        uLCD.printf("Count_R: %d", temp_R);
                
                        dist_L = (circumference/8)*((double)temp_L);
                        dist_R = (circumference/8)*((double)temp_R);
                        
                        uLCD.locate(0,7);
                        uLCD.printf("distance_L: %0.2f cm", dist_L);
                        uLCD.locate(0,8);
                        uLCD.printf("distance_R: %0.2f cm", dist_R);
                        */
                    }
                    else
                    {
                        m_L.speed(0);
                        m_R.speed(0);   
                        wait(0.5);
                        //m_L.speed(sA);
                        //m_R.speed(sB);
                        //wait(4); 
                    }
                }
            }
            
/////////////////////////////////////// Turn Right  ////////////////////////////      
      
            if(i == 'r')
            {
                //speedA = speed_m;
                
                uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Turn Right!");
                m_L.speed(0.65);
                m_R.speed(-0.67); 
                
                count_L = 0;
                count_R = 0;
                
                while(!(uart.readable()))
                {
                    if((count_L < 5)||(count_R < 5))
                    {
                        /*
                        temp_L = count_L;
                        temp_R = count_R;
                        
                        uLCD.locate(0,4);
                        uLCD.printf("Count_L: %d", temp_L);
                
                        uLCD.locate(0,5);
                        uLCD.printf("Count_R: %d", temp_R);
                
                        dist_L = (circumference/8)*((double)temp_L);
                        dist_R = (circumference/8)*((double)temp_R);
                        
                        uLCD.locate(0,7);
                        uLCD.printf("distance_L: %0.2f cm", dist_L);
                        uLCD.locate(0,8);
                        uLCD.printf("distance_R: %0.2f cm", dist_R);
                        */
                    }
                    else
                    {
                        m_L.speed(0);
                        m_R.speed(0);   
                        wait(0.5);
                        //m_L.speed(sA);
                        //m_R.speed(sB);
                        //wait(4); 
                    }
                }
            }
            
            
            ////////////////////////////Tilt to the left //////////////////////////////
            
            if(i == 'a')
            {
                //speedB = speedB - 0.1;
                
                //uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Tilt to the left!");
               
                myled1 = 0;
                myled2 = 1;
                myled3 = 0;
                myled4 = 0;
                
                m_L.speed(sA-0.04);
                m_R.speed(sB+0.02); 
                
                count_L = 0;
                count_R = 0;
                
                
            }
            
            ///////////////////////////Tilt to the right///////////////////////////
            
            if(i == 'z')
            {
                
                //speedB = speedB - 0.1;
                
                //uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Tilt to the right!");
                m_L.speed(sA+0.00);
                m_R.speed(sB-0.05); 
                
                myled1 = 0;
                myled2 = 0;
                myled3 = 0;
                myled4 = 1;
                
                count_L = 0;
                count_R = 0;
                
            }
            
            
            ////////////////Tilt angle to the left(CCW)///////////////
            
            if(i == 'g')
            {
            
                //uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Turn CCW a bit more!");
                
                m_L.speed(0);
                m_R.speed(0.8); 
                wait(0.1);
                m_L.speed(0);
                m_R.speed(0); 
                
                
            }
            
            
            //////////////////Tilt angle to the right (CW)////////////////////////
            
            if(i == 'h')
            {
                //uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Turn CW a bit more!");
                
                m_L.speed(0.8);
                m_R.speed(0); 
                wait(0.1);
                m_L.speed(0);
                m_R.speed(0); 
                
                
                
            }
            
            ///////////////////////////Parked ///////////////////////////
            if(i == 'p')
            {
                uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("stoped");
                wait(0.5);
                m_L.speed(0);
                m_R.speed(0); 
                
                count_L = 0;
                count_R = 0;
                
            }
            
/////////////////////////////////////// Parking #1, On the left ////////////////////////////
        if(i == 'w')
            {
                uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Parking on left! Front Park!");
                
                m_L.speed(sA);
                m_R.speed(sB); 
                wait(0);
                
                m_L.speed(0);
                m_R.speed(0);
                wait(1);
                count_L = 0;
                count_R = 0;
                
                m_L.speed(-0.6);
                m_R.speed(0.65); 
                
                // Step 1: Turn left 90 degrees
                while((count_L < 5)||(count_R < 5))
                {
                    
                }
                m_L.speed(0);
                m_R.speed(0); 
                wait(.5);
                count_L = 0;
                count_R = 0;
                
                
                m_L.speed(0);
                m_R.speed(0); 
                count_L = 0;
                count_R = 0;
                
                
            }
/////////////////////////////////////// Parking #2, On the right ////////////////////////////
        if(i == 'x')
            {
                uLCD.cls();
                uLCD.locate(0,3);
                uLCD.printf("Parking on Right! Front Park!");
                
                m_L.speed(sA);
                m_R.speed(sB); 
                wait(0);
                
                m_L.speed(0);
                m_R.speed(0);
                wait(1);
                count_L = 0;
                count_R = 0;
                
                m_L.speed(0.6);
                m_R.speed(-0.65); 
                
                // Step 1: Turn left 90 degrees
                while((count_L < 5)||(count_R < 5))
                {
                    
                }
                m_L.speed(0);
                m_R.speed(0); 
                wait(.5);
                count_L = 0;
                count_R = 0;
                
                
                m_L.speed(0);
                m_R.speed(0); 
                count_L = 0;
                count_R = 0;
                
                
            }
            
            
        }
        
        else
        {
            myled1 = 0;
        }
        
        
        /*
                
        t.reset();
        t.start();
        count_L = 0;
        count_R = 0;
        
        while(t.read_ms() < 251) { ; }     
        
        t.stop();
        
        long int temp_L = count_L;
        long int temp_R = count_R;
        
        uLCD.locate(0,1);
        uLCD.printf("Count_L: %d", temp_L);
        
        uLCD.locate(0,8);
        uLCD.printf("Count_R: %d", temp_R);
        
        double circumference = 0.065 * 3.1416; // 6 cm wheel diameter * pi 
        
        double rev_L = (double)(temp_L/4);
        double rpm_L = ((double)temp_L)*15;
        double speed_L = (circumference * ((double)temp_L))/4;
        
        double rev_R = (double)(temp_R/4);
        double rpm_R = ((double)temp_R)*15;
        double speed_R = (circumference * ((double)temp_R))/4;
        
        
        uLCD.locate(0,3);
        uLCD.printf("L: %0.2f RPM", rpm_L);
        
        uLCD.locate(0,4);
        uLCD.printf("L: speed: %f m/s", speed_L);
        
        uLCD.locate(0,2);
        uLCD.printf("Ct_nr_L: %d", (count_nr_L/4));
        
        uLCD.locate(0,10);
        uLCD.printf("R: %0.2f RPM", rpm_R);
        
        uLCD.locate(0,11);
        uLCD.printf("R: speed: %f m/s", speed_R);
        
        uLCD.locate(0,9);
        uLCD.printf("Ct_nr_R: %d", (count_nr_R/4));
        
        */
        
        //pc.putc(0xA);
        //pc.putc(0xD);
    }
}