Lorenzo Arrigoni
/
The_Nucleo_Duck
The Nucleo Duck Another simple and amazing version of biped with 3 r/c servos. For STM32F411 Nucleo board
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?
User | Revision | Line number | New 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 |