Diff: main.cpp

Revision:

3:f68eaa68f4ec

Parent:

2:17379e2a6f7d

Child:

4:7f9c9bd9da26

--- a/main.cpp	Sat Nov 23 22:30:04 2013 +0000
+++ b/main.cpp	Sun Nov 24 21:57:52 2013 +0000
@@ -3,1018 +3,1019 @@
 #define CONTROL_PERIOD 0.002 // 500Hz ***
 #define SAVE_PERIOD 0.005 // 200HZ
 
-//think about start up and shut down sequences
+// 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
+};
 
 //control flow
+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
+
 volatile int current_sample = 0;
 volatile int current_loop = 0;
 const int n_samples = 1000;
 const int n_loops = 2;
 
-// 500 x 3 array of degree values
-const int trajectory[1000][3] = {
-1,1,0,
-1,1,0,
-1,1,0,
-1,1,0,
-1,1,0,
-1,1,0,
-1,1,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,0,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,1,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,2,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,3,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,4,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,5,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,6,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,7,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,8,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,9,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,10,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,11,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,12,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-0,0,13,
-1,1,13,
-1,1,13,
-1,1,13,
-1,1,13,
-1,1,14,
-1,1,14,
-1,1,14,
-1,1,14,
-1,1,14,
-1,1,14,
-2,2,14,
-2,2,14,
-2,2,14,
-2,2,14,
-2,2,14,
-2,2,14,
-2,2,14,
-2,2,14,
-3,3,14,
-3,3,14,
-3,3,14,
-3,3,14,
-3,3,14,
-3,3,14,
-4,4,14,
-4,4,14,
-4,4,14,
-4,4,14,
-4,4,14,
-4,4,14,
-5,5,14,
-5,5,14,
-5,5,14,
-5,5,14,
-5,5,14,
-6,6,14,
-6,6,14,
-6,6,14,
-6,6,14,
-6,6,14,
-7,7,14,
-7,7,14,
-7,7,14,
-7,7,14,
-7,7,14,
-8,8,14,
-8,8,14,
-8,8,14,
-8,8,14,
-8,8,14,
-9,9,14,
-9,9,14,
-9,9,14,
-9,9,14,
-9,9,15,
-10,10,15,
-10,10,15,
-10,10,15,
-10,10,15,
-10,10,15,
-11,11,15,
-11,11,15,
-11,11,15,
-11,11,15,
-11,11,15,
-12,12,15,
-12,12,15,
-12,12,15,
-12,12,15,
-12,12,15,
-13,13,15,
-13,13,15,
-13,13,15,
-13,13,15,
-13,13,15,
-14,14,15,
-14,14,15,
-14,14,15,
-14,14,15,
-14,14,15,
-15,15,15,
-15,15,15,
-15,15,14,
-15,15,14,
-15,15,14,
-16,16,14,
-16,16,14,
-16,16,14,
-16,16,14,
-16,16,14,
-17,17,14,
-17,17,14,
-17,17,14,
-17,17,14,
-17,17,14,
-18,18,14,
-18,18,14,
-18,18,14,
-18,18,14,
-18,18,14,
-19,19,14,
-19,19,14,
-19,19,14,
-19,19,14,
-19,19,14,
-20,20,14,
-20,20,14,
-20,20,14,
-20,20,14,
-20,20,14,
-21,21,14,
-21,21,14,
-21,21,14,
-21,21,14,
-21,21,14,
-21,21,14,
-22,22,14,
-22,22,14,
-22,22,14,
-22,22,14,
-23,23,14,
-23,23,14,
-23,23,14,
-23,23,14,
-23,23,14,
-24,24,14,
-24,24,14,
-24,24,14,
-24,24,14,
-24,24,14,
-25,25,14,
-25,25,14,
-25,25,13,
-25,25,13,
-25,25,13,
-26,26,13,
-26,26,13,
-26,26,13,
-26,26,13,
-26,26,13,
-27,27,13,
-27,27,13,
-27,27,13,
-27,27,13,
-27,27,13,
-28,28,12,
-28,28,12,
-28,28,12,
-28,28,12,
-28,28,12,
-29,29,12,
-29,29,12,
-29,29,12,
-29,29,12,
-29,29,12,
-30,30,11,
-30,30,11,
-30,30,11,
-30,30,11,
-30,30,11,
-31,31,11,
-31,31,11,
-31,31,11,
-31,31,11,
-31,31,10,
-32,32,10,
-32,32,10,
-32,32,10,
-32,32,10,
-32,32,10,
-33,33,10,
-33,33,10,
-33,33,9,
-33,33,9,
-33,33,9,
-33,33,9,
-34,34,9,
-34,34,9,
-34,34,9,
-34,34,9,
-34,34,9,
-35,35,8,
-35,35,8,
-35,35,8,
-35,35,8,
-36,36,8,
-36,36,8,
-36,36,8,
-36,36,8,
-36,36,7,
-37,37,7,
-37,37,7,
-37,37,7,
-37,37,7,
-37,37,7,
-38,38,7,
-38,38,7,
-38,38,6,
-38,38,6,
-38,38,6,
-39,39,6,
-39,39,6,
-39,39,6,
-39,39,6,
-39,39,6,
-40,40,5,
-40,40,5,
-40,40,5,
-40,40,5,
-40,40,5,
-40,40,5,
-41,41,5,
-41,41,5,
-41,41,5,
-41,41,4,
-41,41,4,
-42,42,4,
-42,42,4,
-42,42,4,
-42,42,4,
-42,42,4,
-43,43,4,
-43,43,3,
-43,43,3,
-43,43,3,
-43,43,3,
-44,44,3,
-44,44,3,
-44,44,3,
-44,44,3,
-44,44,2,
-45,45,2,
-45,45,2,
-45,45,2,
-45,45,2,
-45,45,2,
-45,45,2,
-46,46,2,
-46,46,2,
-46,46,1,
-46,46,1,
-46,46,1,
-46,46,1,
-47,47,1,
-47,47,1,
-47,47,1,
-47,47,1,
-47,47,0,
-47,47,0,
-47,47,0,
-47,47,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-48,48,0,
-49,49,0,
-49,49,0,
-49,49,0,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-1,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-2,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-3,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-4,
-49,49,-5,
-49,49,-5,
-49,49,-5,
-49,49,-5,
-49,49,-5,
-49,49,-5,
-49,49,-5,
-50,50,-5,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-6,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-7,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-8,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-9,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-10,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-11,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-12,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-13,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-14,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-50,50,-15,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-49,49,-14,
-48,48,-14,
-48,48,-14,
-48,48,-14,
-48,48,-14,
-48,48,-14,
-48,48,-14,
-48,48,-14,
-48,48,-13,
-48,48,-13,
-48,48,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-47,47,-13,
-46,46,-13,
-46,46,-13,
-46,46,-12,
-46,46,-12,
-46,46,-12,
-46,46,-12,
-45,45,-12,
-45,45,-12,
-45,45,-12,
-45,45,-12,
-45,45,-12,
-45,45,-12,
-44,44,-11,
-44,44,-11,
-44,44,-11,
-44,44,-11,
-44,44,-11,
-43,43,-11,
-43,43,-11,
-43,43,-11,
-43,43,-11,
-43,43,-10,
-42,42,-10,
-42,42,-10,
-42,42,-10,
-42,42,-10,
-42,42,-10,
-41,41,-10,
-41,41,-10,
-41,41,-9,
-41,41,-9,
-41,41,-9,
-40,40,-9,
-40,40,-9,
-40,40,-9,
-40,40,-9,
-40,40,-9,
-39,39,-8,
-39,39,-8,
-39,39,-8,
-39,39,-8,
-39,39,-8,
-38,38,-8,
-38,38,-8,
-38,38,-8,
-38,38,-8,
-38,38,-7,
-37,37,-7,
-37,37,-7,
-37,37,-7,
-37,37,-7,
-37,37,-7,
-36,36,-7,
-36,36,-7,
-36,36,-6,
-36,36,-6,
-36,36,-6,
-36,36,-6,
-35,35,-6,
-35,35,-6,
-35,35,-6,
-35,35,-6,
-34,34,-5,
-34,34,-5,
-34,34,-5,
-34,34,-5,
-34,34,-5,
-34,34,-5,
-33,33,-5,
-33,33,-5,
-33,33,-5,
-33,33,-4,
-33,33,-4,
-32,32,-4,
-32,32,-4,
-32,32,-4,
-32,32,-4,
-31,31,-4,
-31,31,-4,
-31,31,-3,
-31,31,-3,
-31,31,-3,
-30,30,-3,
-30,30,-3,
-30,30,-3,
-30,30,-3,
-30,30,-3,
-30,30,-3,
-29,29,-2,
-29,29,-2,
-29,29,-2,
-29,29,-2,
-28,28,-2,
-28,28,-2,
-28,28,-2,
-28,28,-2,
-28,28,-2,
-28,28,-2,
-27,27,-1,
-27,27,-1,
-27,27,-1,
-27,27,-1,
-27,27,-1,
-26,26,-1,
-26,26,-1,
-26,26,-1,
-26,26,-1,
-26,26,-1,
-25,25,-1,
-25,25,-1,
-25,25,-1,
-25,25,0,
-25,25,0,
-24,24,0,
-24,24,0,
-24,24,0,
-24,24,0,
-24,24,0,
-23,23,0,
-23,23,0,
-23,23,0,
-23,23,0,
-23,23,0,
-22,22,0,
-22,22,0,
-22,22,0,
-22,22,0,
-21,21,0,
-21,21,0,
-21,21,0,
-21,21,0,
-21,21,0,
-20,20,0,
-20,20,0,
-20,20,0,
-20,20,0,
-20,20,0,
-19,19,0,
-19,19,0,
-19,19,0,
-19,19,0,
-19,19,0,
-18,18,0,
-18,18,0,
-18,18,0,
-18,18,0,
-18,18,0,
-17,17,0,
-17,17,0,
-17,17,0,
-17,17,0,
-17,17,0,
-16,16,0,
-16,16,0,
-16,16,0,
-16,16,0,
-16,16,0,
-15,15,0,
-15,15,0,
-15,15,0,
-15,15,0,
-15,15,0,
-14,14,0,
-14,14,0,
-14,14,0,
-14,14,0,
-14,14,0,
-13,13,0,
-13,13,0,
-13,13,0,
-13,13,0,
-13,13,0,
-12,12,0,
-12,12,0,
-12,12,0,
-12,12,0,
-12,12,0,
-11,11,0,
-11,11,0,
-11,11,0,
-11,11,0,
-11,11,0,
-10,10,0,
-10,10,0,
-10,10,0,
-10,10,0,
-10,10,0,
-9,9,0,
-9,9,0,
-9,9,0,
-9,9,0,
-9,9,0,
-9,9,0,
-8,8,0,
-8,8,0,
-8,8,0,
-8,8,0,
-8,8,0,
-7,7,0,
-7,7,0,
-7,7,0,
-7,7,0,
-7,7,0,
-6,6,0,
-6,6,0,
-6,6,0,
-6,6,0,
-6,6,0,
-5,5,0,
-5,5,0,
-5,5,0,
-5,5,0,
-5,5,0,
-4,4,0,
-4,4,0,
-4,4,0,
-4,4,0,
-4,4,0,
-4,4,0,
-3,3,0,
-3,3,0,
-3,3,0,
-3,3,0,
-3,3,0,
-3,3,0,
-2,2,0,
-2,2,0,
-2,2,0,
-2,2,0,
-2,2,0,
-2,2,0,
-2,2,0,
-2,2,0,
-1,1,0,
-1,1,0,
-1,1,0
-};
-
 Ticker tick;
 Ticker tock;
 Timer t;
@@ -1074,7 +1075,12 @@
 const float n2d = 3.3333;
 
 const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
-const float stall_current = 8000; //mA
+const float stall_current = 6000; //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
+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;
@@ -1170,10 +1176,22 @@
  }
 
 void control() {
-    rear_n_d = -trajectory[current_sample][rear]*n2d;
-    front_n_d = -trajectory[current_sample][front]*n2d;
-    spine_n_d = -trajectory[current_sample][spine]*n2d;
-
+    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; 
+    }
+    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; 
+    }
+    else{
+        rear_n_d = -trajectory[current_sample][rear]*n2d;
+        front_n_d = -trajectory[current_sample][front]*n2d;
+        spine_n_d = -trajectory[current_sample][spine]*n2d;
+    }
+    
     // rear
     i = read_current(rear);
     w = updateEncoder(rear);      
@@ -1204,9 +1222,23 @@
     if (avg_current[spine] > stall_current){pwm = 0;spine_led=1;}
     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;}
+
+
+
     //step to next control point
-    if (current_sample == n_samples){
-        if (current_loop == n_loops){ //done
+    if (getting_up){
+        if(current_sample == 1000){ 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);
@@ -1215,14 +1247,16 @@
             updateMotor(front,pwm);
             updateMotor(spine,pwm);
         }
+        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{ //end of loop, ready for next
             current_sample = 0;
             current_loop++;
         }
     }
-    else{ //middle of loop
-        current_sample++;
-    }
+    else{ current_sample++;} //middle of running loop
 } 
 
 void save() {
@@ -1237,7 +1271,7 @@
     tock.attach(&save,SAVE_PERIOD);
     t.start();
     
-    while(1) { 
+    while(~kill_test) {
         //DEBUG
 //        pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
     }