bounding - embedded code for bounding robot

Users » calisch » Code » bounding
Sam Calisch / bounding
embedded code for bounding robot
Dependencies: QEI mbed
Fork of bounding by Sam Calisch
Files at this revision
API Documentation at this revision
Revision 4:7f9c9bd9da26, committed 2014-04-20

Comitter:: calisch
Date:: Sun Apr 20 18:01:02 2014 +0000
Parent:: 3:f68eaa68f4ec
Commit message:: hi there;
Changed in this revision

main.cpp
Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Sun Nov 24 21:57:52 2013 +0000
+++ b/main.cpp	Sun Apr 20 18:01:02 2014 +0000
@@ -3,1018 +3,191 @@
 #define CONTROL_PERIOD 0.002 // 500Hz ***
 #define SAVE_PERIOD 0.005 // 200HZ
 
-// 500 x 3 array of degree values
-const float trajectory[1000][3] = {
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,0
+const int n_samples = 165;
+
+// 1000 x 3 array of degree values
+const float trajectory[n_samples][3] = {
+34,10,0,
+34,10,0,
+34,10,0,
+34,10,0,
+34,10,0,
+34,10,-1,
+34,10,-1,
+34,10,-1,
+34,10,-2,
+34,10,-2,
+34,10,-2,
+34,10,-3,
+34,10,-3,
+34,10,-3,
+33,11,-3,
+33,11,-4,
+33,11,-4,
+33,11,-4,
+33,11,-4,
+32,12,-4,
+32,12,-5,
+32,12,-5,
+31,13,-5,
+31,13,-5,
+31,13,-5,
+30,14,-5,
+30,14,-6,
+30,14,-6,
+29,15,-6,
+29,15,-6,
+28,16,-6,
+28,16,-6,
+27,17,-6,
+27,17,-6,
+26,18,-6,
+26,18,-6,
+25,19,-6,
+25,19,-6,
+24,20,-6,
+24,20,-6,
+23,21,-6,
+23,21,-6,
+22,22,-6,
+22,22,-6,
+21,23,-6,
+20,24,-6,
+20,24,-6,
+19,25,-6,
+19,25,-6,
+18,26,-6,
+18,26,-6,
+17,27,-6,
+17,27,-6,
+16,28,-6,
+16,28,-6,
+15,29,-6,
+15,29,-6,
+15,29,-6,
+14,30,-6,
+14,30,-5,
+13,31,-5,
+13,31,-5,
+13,31,-5,
+12,32,-5,
+12,32,-5,
+12,32,-4,
+12,32,-4,
+11,33,-4,
+11,33,-4,
+11,33,-4,
+11,33,-3,
+10,34,-3,
+10,34,-3,
+10,34,-3,
+10,34,-2,
+10,34,-2,
+10,34,-2,
+10,34,-1,
+10,34,-1,
+10,34,-1,
+10,34,0,
+10,34,0,
+10,34,0,
+10,34,0,
+10,34,0,
+10,34,0,
+10,34,0,
+10,34,1,
+10,34,1,
+10,34,1,
+10,34,2,
+10,34,2,
+10,34,2,
+10,34,2,
+10,34,3,
+10,34,3,
+10,34,3,
+11,33,3,
+11,33,4,
+11,33,4,
+11,33,4,
+12,32,4,
+12,32,5,
+12,32,5,
+12,32,5,
+13,31,5,
+13,31,5,
+13,31,5,
+14,30,6,
+14,30,6,
+15,29,6,
+15,29,6,
+15,29,6,
+16,28,6,
+16,28,6,
+17,27,6,
+17,27,6,
+18,26,6,
+18,26,6,
+19,25,6,
+19,25,6,
+20,24,6,
+20,24,6,
+21,23,6,
+22,22,6,
+22,22,6,
+23,21,6,
+23,21,6,
+24,20,6,
+24,20,6,
+25,19,6,
+25,19,6,
+26,18,6,
+26,18,6,
+27,17,6,
+27,17,6,
+28,16,6,
+28,16,6,
+29,15,6,
+29,15,6,
+30,14,6,
+30,14,6,
+30,14,5,
+31,13,5,
+31,13,5,
+31,13,5,
+32,12,5,
+32,12,5,
+32,12,4,
+33,11,4,
+33,11,4,
+33,11,4,
+33,11,3,
+33,11,3,
+34,10,3,
+34,10,3,
+34,10,2,
+34,10,2,
+34,10,2,
+34,10,2,
+34,10,1,
+34,10,1,
+34,10,1,
+34,10,0,
+34,10,0
 };
 
 //control flow
+const int standing_time = 250; //samples at 500Hz, time to stand up
+const float standing_step = 1/(float)standing_time; //recipherical of standing_time
+const int sitting_time = 1000; //samples at 500Hz, time to sit down
+const float sitting_step = 1/(float)sitting_time; //recipherical of sitting time
 volatile int getting_up = 1; //are we currently in the process of standing up
 volatile int getting_down = 0; //are we currently in the process of sitting down
+const float standing_position = 34.0;
+const float bent_position = 10.0;
+const float spine_start = trajectory[0][2];
 
 volatile int current_sample = 0;
 volatile int current_loop = 0;
-const int n_samples = 1000;
-const int n_loops = 2;
+const int n_loops = 12;
 
 Ticker tick;
 Ticker tock;
@@ -1024,25 +197,25 @@
 LocalFileSystem local("data");               // Create the local filesystem under the name "local"
 
 // Declare Three Encoders
-QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING);  // rear leg
-QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING);  // front leg
-QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING);  // spine
+QEI rear_encoder(p23, p14, NC, 1200, QEI::X4_ENCODING);  // rear leg
+QEI front_encoder(p21, p16, NC, 1200, QEI::X4_ENCODING);  // front leg
+QEI spine_encoder(p22, p15, NC, 1200, QEI::X4_ENCODING);  // spine
 
 // Specify pinout
-DigitalOut  rear_motorA(p15);
-DigitalOut  rear_motorB(p16);
-PwmOut      rear_motorPWM(p24);
-AnalogIn    rear_cs(p20);
+DigitalOut  rear_motorA(p7);
+DigitalOut  rear_motorB(p6);
+PwmOut      rear_motorPWM(p26);
+AnalogIn    rear_cs(p18);
 
-DigitalOut  front_motorA(p7);
-DigitalOut  front_motorB(p8);
-PwmOut      front_motorPWM(p25);
-AnalogIn    front_cs(p19);
+DigitalOut  front_motorA(p11);
+DigitalOut  front_motorB(p10);
+PwmOut      front_motorPWM(p24);
+AnalogIn    front_cs(p20);
 
-DigitalOut  spine_motorA(p11);
-DigitalOut  spine_motorB(p12);
-PwmOut      spine_motorPWM(p26);
-AnalogIn    spine_cs(p18);
+DigitalOut  spine_motorA(p9);
+DigitalOut  spine_motorB(p8);
+PwmOut      spine_motorPWM(p25);
+AnalogIn    spine_cs(p19);
 
 //LEDs for current safety
 DigitalOut rear_led(LED1);
@@ -1063,7 +236,7 @@
 volatile int n[3] = {0,0,0}; //encoder values
 volatile int last_n[3] = {0,0,0}; //previous encoder values
 
-volatile float avg_current[3] = {0,0,0}; //integration of current in time
+volatile float avg_current[3] = {0.0,0.0,0.0}; //integration of current in time
 
 // Output Variables
 volatile float pwm = 0;
@@ -1075,25 +248,25 @@
 const float n2d = 3.3333;
 
 const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
-const float stall_current = 6000; //mA
+const float stall_current = 8000; //mA
 
-const float time_out_current = 4000; //mA if avg_current is above, increment the timeout count
-const int time_out_steps = 5; //max steps after which kill the test
+const float time_out_current = 5000; //mA if avg_current is above, increment the timeout count
+const int time_out_steps = 400; //max steps after which kill the test
 volatile int time_out_count[3] = {0,0,0};  //counter for time out
 int kill_test = 0; //kill switch
 
 // Control Parameters
 float rear_Kp = 0.001;
-float rear_Ks_p = 250.0;
-float rear_Ks_d = 25.0;
+float rear_Ks_p = 220.0;
+float rear_Ks_d = 500.0;
 
 float front_Kp = 0.001;
-float front_Ks_p = 250.0;
-float front_Ks_d = 25.0;
+float front_Ks_p = 220.0;
+float front_Ks_d = 500.0;
 
 float spine_Kp = 0.001;
-float spine_Ks_p = 200.0;
-float spine_Ks_d = 20.0;
+float spine_Ks_p = 300.0;
+float spine_Ks_d = 400.0;
 
 float rear_n_d = 0.0*n2d;
 float front_n_d = 0.0*n2d;
@@ -1175,16 +348,26 @@
     return w;
  }
 
+void shutdown(){
+    tick.detach();
+    tock.detach();
+    updateMotor(rear, 0);
+    updateMotor(front, 0);
+    updateMotor(spine, 0);
+    fclose(fp);
+    kill_test = 1;
+}
+
 void control() {
     if(getting_up){
-        rear_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
-        front_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
-        spine_n_d = 0.0; 
+        rear_n_d = -standing_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
+        front_n_d = -bent_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
+        spine_n_d = -spine_start*(standing_step*current_sample)*n2d; 
     }
     else if(getting_down){
-        rear_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
-        front_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
-        spine_n_d = 0.0; 
+        rear_n_d = -standing_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
+        front_n_d = -bent_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
+        spine_n_d = -spine_start*(1-sitting_step*current_sample)*n2d;; 
     }
     else{
         rear_n_d = -trajectory[current_sample][rear]*n2d;
@@ -1199,7 +382,7 @@
     sign = abs(id)/id;
     id = abs(id);
     pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
-    if (avg_current[rear] > stall_current){pwm = 0;rear_led=1;}
+    if (avg_current[rear] > stall_current){pwm = 0;}
     updateMotor(rear,pwm); 
     
     // front
@@ -1209,7 +392,7 @@
     sign = abs(id)/id;
     id = abs(id);
     pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
-    if (avg_current[front] > stall_current){pwm = 0;front_led=1;}
+    if (avg_current[front] > stall_current){pwm = 0;}
     updateMotor(front,pwm); 
     
     // spine
@@ -1219,38 +402,32 @@
     sign = abs(id)/id;
     id = abs(id);
     pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
-    if (avg_current[spine] > stall_current){pwm = 0;spine_led=1;}
+    if (avg_current[spine] > stall_current){pwm = 0;}
     updateMotor(spine,pwm); 
 
     //timeout for motor safety
     if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
     if( avg_current[front] > time_out_current){ time_out_count[front]++;}
     if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
-    if( time_out_count[rear]>time_out_steps){ fclose(fp);kill_test = 1;}
-    if( time_out_count[front]>time_out_steps){ fclose(fp);kill_test = 1;}
-    if( time_out_count[spine]>time_out_steps){ fclose(fp);kill_test = 1;}
+    if( time_out_count[rear]>time_out_steps){ rear_led=1;shutdown();}
+    if( time_out_count[front]>time_out_steps){ front_led=1;shutdown();}
+    if( time_out_count[spine]>time_out_steps){ spine_led=1;shutdown();}
 
 
 
     //step to next control point
     if (getting_up){
-        if(current_sample == 1000){ getting_up = 0; current_sample = 0;} //we're up
+        if(current_sample == standing_time){ getting_up = 0; current_sample = 0;} //we're up
         else{  current_sample++;} //still getting up
     }
     else if(getting_down){
-        if(current_sample == 1000){ //we're down
-            tick.detach();
-            tock.detach();
-            fclose(fp);
-            pwm = 0;
-            updateMotor(rear,pwm); 
-            updateMotor(front,pwm);
-            updateMotor(spine,pwm);
+        if(current_sample == sitting_time){ //we're down
+            shutdown();
         }
         else{current_sample++;}    //still getting down
     }
-    else if (current_sample == n_samples){ //normal operation
-        if (current_loop == n_loops){ getting_down = 1; current_sample=0;} //ready to sit
+    else if (current_sample == n_samples-1){ //normal operation
+        if (current_loop == n_loops-1){ getting_down = 1; current_sample=0;} //ready to sit
         else{ //end of loop, ready for next
             current_sample = 0;
             current_loop++;
@@ -1260,7 +437,8 @@
 } 
 
 void save() {
-    fprintf(fp, "%i %i %i %i %f %f %f\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2]);
+    pc.printf("%i %i %i %i %f %f %f %i\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2], current_sample);
+    pc.printf("%i",rear_encoder.getPulses());
 }
 
 int main() {
@@ -1273,6 +451,7 @@
     
     while(~kill_test) {
         //DEBUG
-//        pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
+        save();
+        //pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
     }
 }
\ No newline at end of file
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Created:	23 Nov 2013
Imports:	1
Forks:	1
Commits:	5
Dependents:	0
Dependencies:	2
Followers:	1
Revision 4:7f9c9bd9da26, committed 2014-04-20

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
