Diff: main.cpp

Revision:

0:41b6e235a1bc

Child:

1:b09deae00682

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 12 13:56:11 2015 +0000
@@ -0,0 +1,123 @@
+//==================================
+//    The Nucleo Duck
+//        v. 1.0
+//==================================
+//    by Lorenzo Arrigoni 
+//      loarri@gmail.com
+//----------------------------------
+//     October 2015
+////////////////////////////////////////////
+
+#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