File content as of revision 10:0fdeecaf366d:

#include "mbed.h"
#include "PwmOut.h"
#include "Timer.h"
#include "defines.h"

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//problems to still solve:
//-Who sets distance and angle back to 0?
//
//to drive a certain distance set the float distance to the desired meters
//to turn set angle to desired degrees rotation with respect to current situation
//
//
//
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

//PIN declarations

PwmOut l_wheel_1(p5);
PwmOut l_wheel_2(p6);
PwmOut r_wheel_1(p25);
PwmOut r_wheel_2(p26);

DigitalOut drive_enable(p7); // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~is this a good pin?
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

I2CSlave slave(p28, p27);

Timer timer;
int state = 1;
float distance = 0; //distance to drive in m
float dist_time = 0;
float meter_time = 2; //seconds for driving a meter
float end_time = 0;

float angle = 0;
float turn_time = 0;
float degree_time = 0.5; //seconds for turning 1 degree
float turn_end_time = 0;

//value declarations
float dc_drive = 0.1;// change for higher speeds
float dc_low = 0;
float dc_high = 1;

//function declarations
void showstate(void);

void sm_drive()
{
    led4 = !led4;
    showstate();
    switch(state)
    {
        case 1:                  //standstill
        
        // motoren stil
        l_wheel_1.write(dc_low);
        l_wheel_2.write(dc_low); 
        r_wheel_1.write(dc_low);
        r_wheel_2.write(dc_low);
        
        //state transitions
        if(distance > 0)
        {
            state = 2;
            timer.start();
            dist_time = distance * meter_time;
            end_time = timer.read() + dist_time;
        }
        if(distance < 0)
        {
            state = 3;
            timer.start();
            dist_time = -distance * meter_time;
            end_time = timer.read() + dist_time;
        }
        
        if(angle > 0)
        {
            state = 4;
            timer.start();
            turn_time = angle * degree_time;
            turn_end_time = timer.read() + turn_time;
        }
    if(angle < 0)
        {
            state = 5;
            timer.start();
            turn_time = -angle * degree_time;
            turn_end_time = timer.read() + turn_time;
        }
     
        
        break;
        
        case 2:                  //drive forward
        l_wheel_1.write(dc_drive);
        l_wheel_2.write(dc_high);
        r_wheel_1.write(dc_drive);
        r_wheel_2.write(dc_high);
        //check time of clock
        if(timer.read() >= end_time)
        {
            state = 1;
            timer.stop();
            distance = 0;
        }
        break;
        
        case 3:                  //drive backward
        l_wheel_1.write(dc_high);
        l_wheel_2.write(dc_drive);
        r_wheel_1.write(dc_high);
        r_wheel_2.write(dc_drive);
        //check time of clock
        if(timer.read() >= end_time)
        {
            state = 1;
            timer.stop();
            distance = 0;
        }
        break;
        
        case 4:                  // turn left
        l_wheel_1.write(dc_high);
        l_wheel_2.write(dc_drive);
        r_wheel_1.write(dc_drive);
        r_wheel_2.write(dc_high);
        
        if(timer.read() >= turn_end_time)
        {
            state = 1;
            timer.stop();
            angle = 0;
        }
        break;
        
        case 5:                  //turn right
        l_wheel_1.write(dc_drive);
        l_wheel_2.write(dc_high);
        r_wheel_1.write(dc_high);
        r_wheel_2.write(dc_drive);
        if(timer.read() >= turn_end_time)
        {
            state = 1;
            timer.stop();
            angle = 0;
        }
        break;
        
        default:
        state = 1;
        break;
    }
}



void showstate()
{
    switch(state)
    {
    case 1:
    led1 = 1;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    break;
    case 2:
    led1 = 0;
    led2 = 1;
    led3 = 0;
    led4 = 0;
    break;
    case 3:
    led1 = 1;
    led2 = 1;
    led3 = 0;
    led4 = 0;
    break;
    case 4:
    led1 = 0;
    led2 = 0;
    led3 = 1;
    led4 = 0;
    break;
    case 5:
    led1 = 1;
    led2 = 0;
    led3 = 1;
    led4 = 0;
    break;
    }
}

int main()
{
    
    l_wheel_1.period(0.00001);
    r_wheel_1.period(0.00001);
    drive_enable = 1;
    
    char buf[10];
    
    slave.address(I2C_ADDR_SLAVE1);
    
    while(1)
    {
        sm_drive();
        
        int i = slave.receive();
         switch (i) {
             case I2CSlave::ReadAddressed:
                 slave.write("hallo\n\r", strlen("hallo\n\r") + 1); // Includes null char
                 printf("Write Slave!\n\r");
                 break;
             case I2CSlave::WriteGeneral:
                 slave.read(buf, 10);
                 printf("Read G: %i\n\r", buf);
                 break;
             case I2CSlave::WriteAddressed:
                 slave.read(buf, 10);
                 switch(buf[0]) {
                     case I2C_COMMAND_STOP:
                        distance = 0;
                        angle = 0;
                     break;
                     case I2C_COMMAND_DRIVE:
                        distance = buf[1];
                        printf("COMMAND: DRIVE: %i meters\n\r", buf[1]);
                     break;
                     case I2C_COMMAND_ROTATE:
                        angle = buf[1];
                        printf("COMMAND: ROTATE: %i degrees\n\r", buf[1]);
                }
                 break;
         }
         for(int i = 0; i < 10; i++) buf[i] = 0;    // Clear buffer
    }
    
}