File content as of revision 4:a3720ed072d2:

#include "mbed.h"
#include "MODSERIAL/MODSERIAL.h"
#include "SerialComm/SerialComm.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define USHRT_MAX 65535
#define POWER_OFF 32768
#define BACKWARD_MAX 0
#define FORWARD_MAX USHRT_MAX

MODSERIAL pcSerial(USBTX, USBRX);

// Initialize a pins t//o perform analog and digital output fucntions
AnalogOut  aout(p18);
PwmOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

PwmOut motorPwm(p26);
AnalogOut motorAna(p18);
DigitalOut motorEnable(p12);
Ticker aTimer;
AnalogIn potentiometer(p20);
DigitalIn potentiometerEnable(p21);

//Serial pc(USBTX, USBRX);
float voltage;

SerialComm serialComm(&pcSerial);

bool readyToSendNext;
/*
void timerFcn()
{
    readyToSendNext = true;
}
*/

int main(void)
{
    // DEFINE the operation modes
    bool messageMode = false;
    bool analogMode = true;
    led1 = 0.0f;
    led2 = 0;
    led3 = 0;
    motorEnable = 0; // turn off motor
    motorAna.write_u16(POWER_OFF); // set motor pwm to zero
    motorPwm.period_us(200);
    motorPwm = 0.5f;

    uint16_t motorCommandReceived = POWER_OFF;
    uint16_t motorCommand = POWER_OFF;
    uint16_t motorPositionMeas = POWER_OFF;
    float motorCommandFloat = 0.5f;
    pcSerial.baud(115200);
    pcSerial.printf("Start!\n");

    readyToSendNext = false;
    /*
    aTimer.attach(&timerFcn, 0.002); // the address of the function to be attached (flip) and the interval (2 seconds)
    */
    while(1) {
        if(serialComm.checkIfNewMessage()) {
            // read message from PC in order to clear the buffer
            motorCommandReceived = serialComm.getUnsignedShort();

            // -- ENABLE OR DISABLE MOTOR --
            motorEnable = potentiometerEnable; // set led2 value based on switch of potentiometer
            led2 = motorEnable;

            // -- PC OR POTENTIOMETER Control Mode--
            if(messageMode) { //control through pc
                if(analogMode) { //analog
                    motorAna.write_u16(motorCommandReceived);
                } else { //PWM
                    motorCommandFloat = ((float) motorCommandReceived)/ 65535.0f;
                    motorPwm.write( motorCommandFloat * 0.79f + 0.105f);
                    led1 = motorCommandFloat; // drive led1 brightness accordingly
                }
            } else { //potentiometer controlled
                motorCommand = potentiometer.read_u16();
                if(analogMode) { //analog
                    motorAna.write_u16(motorCommand);
                } else { //PWM
                    motorCommandFloat = ((float) motorCommand) / 65535.0f;
                    motorPwm.write( motorCommandFloat * 0.79f + 0.105f);
                    led1 = motorCommandFloat; // drive led1 brightness accordingly
                }
            }

            readyToSendNext = true;
        }

        if(readyToSendNext == true) {
            motorPositionMeas = motorCommandReceived;
            serialComm.sendUnsignedShort(motorPositionMeas);
            readyToSendNext = false;
        }

    }
}