embedded code for bounding robot
Fork of bounding by
Diff: main.cpp
- Revision:
- 4:7f9c9bd9da26
- Parent:
- 3:f68eaa68f4ec
diff -r f68eaa68f4ec -r 7f9c9bd9da26 main.cpp --- 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