Lorenzo Arrigoni / Mbed 2 deprecated The_Nucleo_Duck

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Servo mbed

main.cpp@0:41b6e235a1bc, 2015-11-12 (annotated)

Committer:
loarri
Date:
Thu Nov 12 13:56:11 2015 +0000
Revision:
0:41b6e235a1bc
Child:
1:b09deae00682
The Nucleo Duck; another version of simple but amazing biped robot with 3 servos.; For STM Nucleo boards

Who changed what in which revision?

UserRevisionLine numberNew contents of line
loarri 0:41b6e235a1bc 1 //==================================
loarri 0:41b6e235a1bc 2 // The Nucleo Duck
loarri 0:41b6e235a1bc 3 // v. 1.0
loarri 0:41b6e235a1bc 4 //==================================
loarri 0:41b6e235a1bc 5 // by Lorenzo Arrigoni
loarri 0:41b6e235a1bc 6 // loarri@gmail.com
loarri 0:41b6e235a1bc 7 //----------------------------------
loarri 0:41b6e235a1bc 8 // October 2015
loarri 0:41b6e235a1bc 9 ////////////////////////////////////////////
loarri 0:41b6e235a1bc 10
loarri 0:41b6e235a1bc 11 #include "mbed.h"
loarri 0:41b6e235a1bc 12 #include "Servo.h"
loarri 0:41b6e235a1bc 13
loarri 0:41b6e235a1bc 14 #define SPEED 10 //ms 10 is default
loarri 0:41b6e235a1bc 15
loarri 0:41b6e235a1bc 16 #define BALANCE_SERVO_PIN D3
loarri 0:41b6e235a1bc 17 #define RIGHT_FOOT_SERVO_PIN D6
loarri 0:41b6e235a1bc 18 #define LEFT_FOOT_SERVO_PIN D5
loarri 0:41b6e235a1bc 19
loarri 0:41b6e235a1bc 20 #define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
loarri 0:41b6e235a1bc 21 * it will have an impact on code-size and power consumption. */
loarri 0:41b6e235a1bc 22
loarri 0:41b6e235a1bc 23 #if NEED_CONSOLE_OUTPUT
loarri 0:41b6e235a1bc 24 //Serial usb(USBTX, USBRX); // tx, rx
loarri 0:41b6e235a1bc 25 Serial pc(USBTX, USBRX);
loarri 0:41b6e235a1bc 26 //extern Serial pc;
loarri 0:41b6e235a1bc 27 #define DEBUG(...) { pc.printf(__VA_ARGS__); }
loarri 0:41b6e235a1bc 28 #else
loarri 0:41b6e235a1bc 29 #define DEBUG(...) /* nothing */
loarri 0:41b6e235a1bc 30 #endif /* #if NEED_CONSOLE_OUTPUT */
loarri 0:41b6e235a1bc 31
loarri 0:41b6e235a1bc 32
loarri 0:41b6e235a1bc 33
loarri 0:41b6e235a1bc 34 DigitalOut myled(LED1);
loarri 0:41b6e235a1bc 35 Servo leftFootServo(LEFT_FOOT_SERVO_PIN);
loarri 0:41b6e235a1bc 36 Servo rightFootServo(RIGHT_FOOT_SERVO_PIN);
loarri 0:41b6e235a1bc 37 Servo balanceServo(BALANCE_SERVO_PIN);
loarri 0:41b6e235a1bc 38
loarri 0:41b6e235a1bc 39
loarri 0:41b6e235a1bc 40 float map(float in, float inMin, float inMax, float outMin, float outMax) {
loarri 0:41b6e235a1bc 41 // check it's within the range
loarri 0:41b6e235a1bc 42 if (inMin<inMax) {
loarri 0:41b6e235a1bc 43 if (in <= inMin)
loarri 0:41b6e235a1bc 44 return outMin;
loarri 0:41b6e235a1bc 45 if (in >= inMax)
loarri 0:41b6e235a1bc 46 return outMax;
loarri 0:41b6e235a1bc 47 } else { // cope with input range being backwards.
loarri 0:41b6e235a1bc 48 if (in >= inMin)
loarri 0:41b6e235a1bc 49 return outMin;
loarri 0:41b6e235a1bc 50 if (in <= inMax)
loarri 0:41b6e235a1bc 51 return outMax;
loarri 0:41b6e235a1bc 52 }
loarri 0:41b6e235a1bc 53 // calculate how far into the range we are
loarri 0:41b6e235a1bc 54 float scale = (in-inMin)/(inMax-inMin);
loarri 0:41b6e235a1bc 55 // calculate the output.
loarri 0:41b6e235a1bc 56 return outMin + scale*(outMax-outMin);
loarri 0:41b6e235a1bc 57 }
loarri 0:41b6e235a1bc 58
loarri 0:41b6e235a1bc 59
loarri 0:41b6e235a1bc 60 int main ()
loarri 0:41b6e235a1bc 61 {
loarri 0:41b6e235a1bc 62 //set initial values
loarri 0:41b6e235a1bc 63 myled =0 ;
loarri 0:41b6e235a1bc 64 balanceServo.write(map(90,0,180,0,1.0));
loarri 0:41b6e235a1bc 65 leftFootServo.write(map(60,0,180,0,1.0));
loarri 0:41b6e235a1bc 66 rightFootServo.write(map(60,0,180,0,1.0));
loarri 0:41b6e235a1bc 67 wait_ms(3000); //3 second wait_ms
loarri 0:41b6e235a1bc 68 myled= 1;
loarri 0:41b6e235a1bc 69 int incr = 3 ;
loarri 0:41b6e235a1bc 70
loarri 0:41b6e235a1bc 71 while (1) {
loarri 0:41b6e235a1bc 72 int posFootServo = 60; //For build, set feet straight ahead (servo arms pointing straight outward) at 90 degrees servo setting (1500 usecs).
loarri 0:41b6e235a1bc 73 int posBalanceServo = 90; //For build, set body at no tilt (servo arm horizontal) at 90 degrees servo setting (1500 usecs).
loarri 0:41b6e235a1bc 74 int round = 0;
loarri 0:41b6e235a1bc 75
loarri 0:41b6e235a1bc 76 for (round = 1 ; round <= 120 ; round++)
loarri 0:41b6e235a1bc 77 {
loarri 0:41b6e235a1bc 78 if (round >= 1 && round <= 60)
loarri 0:41b6e235a1bc 79 {
loarri 0:41b6e235a1bc 80 posFootServo++;
loarri 0:41b6e235a1bc 81
loarri 0:41b6e235a1bc 82 if (round < 15)
loarri 0:41b6e235a1bc 83 posBalanceServo=posBalanceServo+incr ;
loarri 0:41b6e235a1bc 84 else if (round > 40 && round < 50)
loarri 0:41b6e235a1bc 85 posBalanceServo = posBalanceServo - (2 * incr);
loarri 0:41b6e235a1bc 86 else if (round > 49)
loarri 0:41b6e235a1bc 87 posBalanceServo=posBalanceServo - incr;
loarri 0:41b6e235a1bc 88 }
loarri 0:41b6e235a1bc 89
loarri 0:41b6e235a1bc 90 if (round >= 61 && round <= 120)
loarri 0:41b6e235a1bc 91 {
loarri 0:41b6e235a1bc 92 posFootServo--;
loarri 0:41b6e235a1bc 93
loarri 0:41b6e235a1bc 94 if (round < 75)
loarri 0:41b6e235a1bc 95 posBalanceServo=posBalanceServo-incr;
loarri 0:41b6e235a1bc 96 else if (round > 100 && round < 110)
loarri 0:41b6e235a1bc 97 posBalanceServo = posBalanceServo + ( 2 * incr) ;
loarri 0:41b6e235a1bc 98 else if (round > 109)
loarri 0:41b6e235a1bc 99 posBalanceServo= posBalanceServo + incr;
loarri 0:41b6e235a1bc 100 }
loarri 0:41b6e235a1bc 101
loarri 0:41b6e235a1bc 102
loarri 0:41b6e235a1bc 103
loarri 0:41b6e235a1bc 104 //set servo positions:
loarri 0:41b6e235a1bc 105
loarri 0:41b6e235a1bc 106 //balance servo: exponential asympote wave shape (steep change at first then tapering flat).
loarri 0:41b6e235a1bc 107 // Shifted in phase from foot servo to help weight shift.
loarri 0:41b6e235a1bc 108 // full range is +/-22 degrees, total swing 44 degrees.
loarri 0:41b6e235a1bc 109
loarri 0:41b6e235a1bc 110 DEBUG("Posizione servo centrale(%i): %i\n\r",round, posBalanceServo);
loarri 0:41b6e235a1bc 111
loarri 0:41b6e235a1bc 112 balanceServo.write(map(posBalanceServo,0,180,0,1.0));
loarri 0:41b6e235a1bc 113
loarri 0:41b6e235a1bc 114 //foot servo: triangle wave shape: both feet ramp from 60 degrees to 120 and back
loarri 0:41b6e235a1bc 115 leftFootServo.write(map(posFootServo,0,180,0,1.0));
loarri 0:41b6e235a1bc 116 rightFootServo.write(map(posFootServo,0,180,0,1.0));
loarri 0:41b6e235a1bc 117
loarri 0:41b6e235a1bc 118 wait_ms(SPEED); //10 ms wait_ms seems about right w/
loarri 0:41b6e235a1bc 119
loarri 0:41b6e235a1bc 120 } //end for (round)
loarri 0:41b6e235a1bc 121 } //end for while
loarri 0:41b6e235a1bc 122
loarri 0:41b6e235a1bc 123 } //end for main