Sam Calisch
embedded code for bounding robot
Fork of
bounding
by 
Sam Calisch
Revision 2:17379e2a6f7d, committed 2013-11-23
- Comitter:
- calisch
- Date:
- Sat Nov 23 22:30:04 2013 +0000
- Parent:
- 1:e549754ca234
- Child:
- 3:f68eaa68f4ec
- Commit message:
- detach tickers, clean up, testing
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Nov 23 21:16:12 2013 +0000
+++ b/main.cpp Sat Nov 23 22:30:04 2013 +0000
@@ -5,503 +5,1009 @@
//think about start up and shut down sequences
+//control flow
+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 float trajectory[500][3] = {
+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,
-2,2,0,
-2,2,0,
-3,3,0,
-3,3,0,
-3,3,0,
-4,4,0,
-4,4,0,
-5,5,0,
-5,5,0,
-6,6,0,
-6,6,0,
-7,7,0,
-7,7,0,
-7,7,0,
-8,8,0,
-8,8,0,
-9,9,0,
-9,9,0,
-10,10,0,
-10,10,0,
-11,11,0,
-11,11,0,
-11,11,0,
-12,12,0,
-12,12,0,
-13,13,0,
-13,13,0,
-14,14,0,
-14,14,0,
-14,14,0,
-15,15,0,
-15,15,0,
-16,16,0,
-16,16,0,
-17,17,0,
-17,17,0,
-18,18,0,
-18,18,0,
-18,18,0,
-19,19,0,
-19,19,0,
-20,20,0,
-20,20,0,
-21,21,0,
-21,21,0,
-22,22,0,
-22,22,0,
-22,22,0,
-23,23,0,
-23,23,0,
-24,24,0,
-24,24,0,
-25,25,0,
-25,25,0,
-25,25,0,
-26,26,0,
-26,26,0,
-27,27,0,
-27,27,0,
-28,28,0,
-28,28,0,
-29,29,0,
-29,29,0,
-29,29,0,
-30,30,0,
-30,30,0,
-31,31,0,
-31,31,0,
-32,32,0,
-32,32,0,
-33,33,0,
-33,33,0,
-33,33,0,
-34,34,0,
-34,34,0,
-35,35,0,
-35,35,0,
-36,36,0,
-36,36,0,
-36,36,0,
-37,37,0,
-37,37,0,
-38,38,0,
-38,38,0,
-39,39,0,
-39,39,0,
-40,40,0,
-40,40,0,
-40,40,0,
-41,41,0,
-41,41,0,
-42,42,0,
-42,42,0,
-43,43,0,
-43,43,0,
-44,44,0,
-44,44,0,
-44,44,0,
-45,45,0,
-45,45,0,
-46,46,0,
-46,46,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,0,
+0,0,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,
-50,50,0,
-50,50,0,
-51,51,0,
-51,51,0,
-51,51,0,
-52,52,0,
-52,52,0,
-53,53,0,
-53,53,0,
-53,53,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-55,55,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-54,54,0,
-53,53,0,
-53,53,0,
-53,53,0,
-52,52,0,
-52,52,0,
-51,51,0,
-51,51,0,
-51,51,0,
-50,50,0,
-50,50,0,
-49,49,0,
-49,49,0,
-48,48,0,
-48,48,0,
-47,47,0,
-47,47,0,
-47,47,0,
-46,46,0,
-46,46,0,
-45,45,0,
-45,45,0,
-44,44,0,
-44,44,0,
-43,43,0,
-43,43,0,
-43,43,0,
-42,42,0,
-42,42,0,
-41,41,0,
-41,41,0,
-40,40,0,
-40,40,0,
-40,40,0,
-39,39,0,
-39,39,0,
-38,38,0,
-38,38,0,
-37,37,0,
-37,37,0,
-36,36,0,
-36,36,0,
-36,36,0,
-35,35,0,
-35,35,0,
-34,34,0,
-34,34,0,
-33,33,0,
-33,33,0,
-33,33,0,
-32,32,0,
-32,32,0,
-31,31,0,
-31,31,0,
-30,30,0,
-30,30,0,
-29,29,0,
-29,29,0,
-29,29,0,
-28,28,0,
-28,28,0,
-27,27,0,
-27,27,0,
-26,26,0,
-26,26,0,
-25,25,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,
@@ -537,6 +1043,11 @@
PwmOut spine_motorPWM(p26);
AnalogIn spine_cs(p18);
+//LEDs for current safety
+DigitalOut rear_led(LED1);
+DigitalOut front_led(LED2);
+DigitalOut spine_led(LED3);
+
//number domains for abstraction
const int rear = 0;
const int front = 1;
@@ -562,8 +1073,8 @@
const int Vs = 18; // [V]
const float n2d = 3.3333;
-const float integ_alpha = .5; //peristence of current integration. 0->all past, 1->all present
-const float stall_current = 5; //Amps
+const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
+const float stall_current = 8000; //mA
// Control Parameters
float rear_Kp = 0.001;
@@ -585,11 +1096,6 @@
float front_w_d = 0;
float spine_w_d = 0;
-//control flow
-volatile int current_sample = 0;
-volatile int current_loop = 0;
-const int n_samples = 500;
-const int n_loops = 1;
FILE *fp = fopen("/data/out.txt", "w"); // Open "out.txt" on the local file system for writing
@@ -618,7 +1124,7 @@
if (power > 1) { power = 1; }
if (power < 0) { power = 0; }
- if (which_motor == 1) { // rear
+ if (which_motor == rear) { // rear
if (dir == 1) {
rear_motorA = 0;
rear_motorB = 1;
@@ -627,7 +1133,7 @@
rear_motorB = 0;
}
rear_motorPWM.write(power);
- } else if (which_motor == 2) { // front
+ } else if (which_motor == front) { // front
if (dir == 1) {
front_motorA = 0;
front_motorB = 1;
@@ -636,7 +1142,7 @@
front_motorB = 0;
}
front_motorPWM.write(power);
- } else if (which_motor == 3) { // spine
+ } else if (which_motor == spine) { // spine
if (dir == 1) {
spine_motorA = 0;
spine_motorB = 1;
@@ -675,7 +1181,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;}
+ if (avg_current[rear] > stall_current){pwm = 0;rear_led=1;}
updateMotor(rear,pwm);
// front
@@ -685,7 +1191,7 @@
sign = abs(id)/id;
id = abs(id);
pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
- if (avg_current[rear] > stall_current){pwm = 0;}
+ if (avg_current[front] > stall_current){pwm = 0;front_led=1;}
updateMotor(front,pwm);
// spine
@@ -695,16 +1201,19 @@
sign = abs(id)/id;
id = abs(id);
pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
- if (avg_current[rear] > stall_current){pwm = 0;}
+ if (avg_current[spine] > stall_current){pwm = 0;spine_led=1;}
updateMotor(spine,pwm);
//step to next control point
if (current_sample == n_samples){
if (current_loop == n_loops){ //done
+ tick.detach();
+ tock.detach();
fclose(fp);
pwm = 0;
updateMotor(rear,pwm);
updateMotor(front,pwm);
+ updateMotor(spine,pwm);
}
else{ //end of loop, ready for next
current_sample = 0;
@@ -714,11 +1223,10 @@
else{ //middle of loop
current_sample++;
}
-
}
void save() {
- fprintf(fp, "%i %i %i %i\n", t.read_ms(), n[rear], n[front], n[spine]);
+ 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]);
}
int main() {