Lorenzo Arrigoni / Mbed 2 deprecated The_Nucleo_Duck

The Nucleo Duck Another simple and amazing version of biped with 3 r/c servos. For STM32F411 Nucleo board

Dependencies:   Servo mbed

main.cpp@1:b09deae00682, 2015-11-30 (annotated)

Committer:
loarri
Date:
Mon Nov 30 15:18:28 2015 +0000
Revision:
1:b09deae00682
Parent:
0:41b6e235a1bc 
comments updated

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