File content as of revision 10:4d5af6e84c7a:

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

/* THIS IS IMPLEMENTED IN THE BIG CODE, DO NOT CHANGE IT HERE, BUT THERE!!! */

//initial allocations
DigitalOut dirpin_1(D4); // for translatie
DigitalOut dirpin_2(D7); // for rotatie
PwmOut pwmpin_1(D5);
PwmOut pwmpin_2(D6);
QEI Encoder1(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;
float speed = 0.70;
int dir = 0;

// Functions 
int Counts1(volatile int& a) // a = counts1
{
    counts1_prev = a;
    a =  Encoder2.getPulses();
    return a;
    }
    
int Counts2(volatile int& a) // a = counts2
{
    counts2_prev = a;
    a = Encoder1.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_1.write(a);
    pwmpin_1 = b;
    }
    
// Stop translation                            
void translation_stop()
{ 
    pwmpin_1 = 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;
    } 

// Calibration of translation
void calibration_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();    
                pos_store(Counts1(counts1)); 
                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
        }
    }
    
void calibration_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);
    }
    
void MOTOR_CAL()
{
    // translation
    calibration_translation();
    
    pc.printf("before wait\r\n");
    wait(3.0);
    
    // rotation
    calibration_rotation();
    
    pc.printf("Motor calibration done");
    }
    

// main function
int main()
{
    pc.baud(115200);                  
    pc.printf("Start\r\n");
    pwmpin.period_us(60);
    
    MOTOR_CAL();
       
    }