File content as of revision 1:b09deae00682:

//==================================
//    The Nucleo Duck
//        v. 1.0
//==================================
//    by Lorenzo Arrigoni 
//      laelectronics@libero.it
//----------------------------------
//     October 2015
/////////////////////////////////////////
// This is a funny biped robot done with 3
// r/c/ servos and driven by Nucleo F411RE
// board from STM.
//
/////////////////////////////////////////

#include "mbed.h"
#include "Servo.h"

#define SPEED 10 //ms 10 is default 

#define BALANCE_SERVO_PIN D3
#define RIGHT_FOOT_SERVO_PIN D6
#define LEFT_FOOT_SERVO_PIN D5

#define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
                            * it will have an impact on code-size and power consumption. */

#if NEED_CONSOLE_OUTPUT
  //Serial usb(USBTX, USBRX); // tx, rx
  Serial pc(USBTX, USBRX);
  //extern Serial  pc;
  #define DEBUG(...) { pc.printf(__VA_ARGS__); }
#else
  #define DEBUG(...) /* nothing */
#endif /* #if NEED_CONSOLE_OUTPUT */



DigitalOut myled(LED1);
Servo leftFootServo(LEFT_FOOT_SERVO_PIN);
Servo rightFootServo(RIGHT_FOOT_SERVO_PIN);
Servo balanceServo(BALANCE_SERVO_PIN);


float map(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);
}


int main () 
{
  //set initial values
  myled =0 ;
  balanceServo.write(map(90,0,180,0,1.0)); 
  leftFootServo.write(map(60,0,180,0,1.0)); 
  rightFootServo.write(map(60,0,180,0,1.0));
  wait_ms(3000); //3 second wait_ms
  myled= 1;
  int incr = 3 ;

while (1) {
  int posFootServo = 60; //For build, set feet straight ahead (servo arms pointing straight outward) at 90 degrees servo setting (1500 usecs).
  int posBalanceServo = 90; //For build, set body at no tilt (servo arm horizontal) at 90 degrees servo setting (1500 usecs).
  int round = 0;
  
  for (round = 1 ; round <= 120 ; round++)
  {
    if (round >= 1 && round <= 60)
    {
      posFootServo++;
      
      if (round < 15)
        posBalanceServo=posBalanceServo+incr ;
      else if (round > 40 && round < 50)
        posBalanceServo = posBalanceServo - (2 * incr);
      else if (round > 49)
        posBalanceServo=posBalanceServo - incr;
    }

    if (round >= 61 && round <= 120)
    {
      posFootServo--;
      
      if (round < 75)
        posBalanceServo=posBalanceServo-incr;
      else if (round > 100 && round < 110)
        posBalanceServo = posBalanceServo + ( 2 * incr) ;
      else if (round > 109)
        posBalanceServo= posBalanceServo + incr;
    }
  
  

    //set servo positions:
    
    //balance servo: exponential asympote wave shape (steep change at first then tapering flat).  
    //  Shifted in phase from foot servo to help weight shift.
    //  full range is +/-22 degrees, total swing 44 degrees.

    DEBUG("Posizione servo centrale(%i): %i\n\r",round, posBalanceServo);

    balanceServo.write(map(posBalanceServo,0,180,0,1.0)); 
    
    //foot servo: triangle wave shape: both feet ramp from 60 degrees to 120 and back
    leftFootServo.write(map(posFootServo,0,180,0,1.0)); 
    rightFootServo.write(map(posFootServo,0,180,0,1.0)); 
    
    wait_ms(SPEED); //10 ms wait_ms seems about right w/ 

  } //end for (round)
  } //end for while

} //end for main