embedded code for bounding robot
Fork of bounding by
Diff: main.cpp
- Revision:
- 2:17379e2a6f7d
- Parent:
- 1:e549754ca234
- Child:
- 3:f68eaa68f4ec
--- 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() {