embedded code for bounding robot
Fork of bounding by
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); }