File content as of revision 7:32caebeb5c3a:

#include "mbed.h"
#include "QEI.h"
#define SERIAL_BAUD 115200

//initial allocations
DigitalOut dirpin(D4); // for translatie
DigitalOut dirpin_2(D7); // for rotatie
PwmOut pwmpin(D5);
PwmOut pwmpin_2(D6);
QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);

Serial pc(USBTX,USBRX);

DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
DigitalIn button_wait(SW3); // button for wait mode, on mbed 
DigitalIn button_demo(D2); // button for demo mode, on bioshield 

// Volatiles
volatile int counts1_prev = 0;
volatile int counts2_prev = 0;
volatile int counts1;
volatile int counts2;
volatile double tower_1_position = 0.1; // the tower which he reaches first
volatile double tower_end_position = 0.1; // the tower which he reaches second
volatile double rotation_start_position = 0.1; // the position where the rotation will remain
volatile double position;

// Functions (2 functions not finished)
int Counts1(volatile int& a) // a = counts1
{
    counts1_prev = a;
    a =  Encoder.getPulses();
    return a;
    }
    
int Counts2(volatile int& a) // a = counts2
{
    counts2_prev = a;
    a = Encoder2.getPulses(); 
    return a;
    }

void pos_store(int a){ //store position in counts to know count location of the ends of bridge

    if (tower_1_position == 0.1){
        tower_1_position = a;
        }
    else if (tower_end_position == 0.1){
        tower_end_position = a;
        }
    else if (rotation_start_position == 0.1){
        rotation_start_position = a;
        }    
    }

// Start translation   
void translation_start(int a, float b) // a = dir , b = speed 
{ 
    dirpin.write(a);
    pwmpin = b;
    }
    
// Stop translation                            
void translation_stop()
{ 
    pwmpin = 0.0;
    }
    
// Start rotation         
void rotation_start(int a, float b)
{ 
    dirpin_2.write(a);
    pwmpin_2 = b;
    }
 
// Stop rotation    
void rotation_stop()
{ 
    pwmpin_2 = 0.0;
    } 


// main function
int main()
{
    pc.baud(115200);                  
    pc.printf("Start\r\n");
    
    pwmpin.period_us(60);
    
    // parameters  
    float speed = 0.70;
    int dir = 0;
    
    // translation
    for(int m = 1; m <= 2; m++) // to do each direction one time
    {
        pc.printf("\r\nTranslatie loop\r\n");
        translation_start(dir,speed); 
        pc.printf("Direction = %i\r\n", dir);
        
        bool g = true; // to make a condition for the while loop
        while (g == true)
        {
            if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
            {
                translation_stop();    
                position = Counts1(counts1);
                pos_store(Counts1(counts1)); // IS THIS NEEDED??
                pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
                dir = dir + 1;
                                
                g = false; // to end the while loop
                }
                
            wait(0.01); 
            }
        
        wait(1.5); // wait 3 seconds before next round of translation/rotation
        } 
    pc.printf("before wait\r\n");
    wait(10.0);
    
    // rotation
    rotation_start(dir, speed);
    pc.printf("\r\nRotatie start\r\n");

    bool f = true; // condition for while loop
    while(f == true)
    {
        if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
        {
            rotation_stop();
            
            f = false; // to end the while loop
            }
        
        wait(0.01);
        }
        
    pos_store(Counts2(counts2));
    pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
    
    pc.printf("Motor calibration done");
    
        
    }