#include "mbed.h"
#include "max32630fthr.h"
MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
#define FORWARD 0x1
/*#define BACKWARD 0x2
#define LEFTTURN 0x3
#define POINTLEFTTURN 0x4
#define RIGHTTURN 0x5
#define POINTRIGHTTURN 0x6*/
void move(int,float,float);
void forward(int,float);
/*void backward(int,float);
void leftTurn(int,float);
void pointleftTurn(int,float);
void rightTurn(int,float);
void pointrightTurn(int,float); */
float speedmap(float,float,float,float,float) 
void stop(void);
DigitalOut M1A(P5_0);
DigitalOut M1B(P5_1);
DigitalOut M2A(P5_6);
DigitalOut M2B(P5_5);
PwmOut Enable(P4_0);
int main() 
{
 while (true) 
 { 
move(FORWARD,50,5);
stop();
wait(3);
 }
}
void move(int direction,float speed,float time)/*Function for Robot Navigation*/
{
speed = speedmap(speed, 0, 100, 0, 0.1);
if(direction==FORWARD)
forward(speed,time);
/*else if(direction==BACKWARD)
backward(speed,time);
else if(direction==LEFTTURN)
leftTurn(speed,time);
else if(direction==POINTLEFTTURN)
pointleftTurn(speed,time);
else if(direction==RIGHTTURN)
rightTurn(speed,time);
else if(direction==POINTRIGHTTURN)
pointrightTurn(speed,time);*/
}
void forward(int speed,float time)/*Function definition to move forward*/
{
M1A = 1; 
M1B = 0; 
M2A = 1; 
M2B = 0; 
Enable.period(0.1f); 
Enable.pulsewidth(speed); 
wait(time);
void stop();
}
void stop()/*Function definition to stop*/
{
M1A = 0; 
M1B = 0; 
M2A = 0; 
M2B = 0; 
Enable.period(0.1f); 
Enable.pulsewidth(0.00f); 
}
float speedmap(float in, float inMin, float inMax, float outMin, float outMax) 
{
  // check it's within the range
  if (inMin<inMax) { 
    if (in <= inMin) 
      return outMin;
    if (in >= inMax)
      return outMax;
  } else {  // cope with input range being backwards.
    if (in >= inMin) 
      return outMin;
    if (in <= inMax)
      return outMax;
  }
  // calculate how far into the range we are
  float scale = (in-inMin)/(inMax-inMin);
  // calculate the output.
  return outMin + scale*(outMax-outMin);
}