Martin Smith
Forward Kinematics code for all four of E.R.I.C.'s legs...
main.cpp@2:a51b150e4189, 2011-10-08 (annotated)
- Committer:
- ms523
- Date:
- Sat Oct 08 16:15:32 2011 +0000
- Revision:
- 2:a51b150e4189
- Parent:
- 1:937018869855
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ms523 | 0:c1e3b0d03727 | 1 | #include "FK.h" |
| ms523 | 0:c1e3b0d03727 | 2 | |
| ms523 | 2:a51b150e4189 | 3 | Serial pc(USBTX,USBRX); |
| ms523 | 2:a51b150e4189 | 4 | Timer t; |
| ms523 | 2:a51b150e4189 | 5 | |
| ms523 | 2:a51b150e4189 | 6 | int main() { |
| ms523 | 2:a51b150e4189 | 7 | |
| ms523 | 2:a51b150e4189 | 8 | Positions pos; |
| ms523 | 2:a51b150e4189 | 9 | |
| ms523 | 2:a51b150e4189 | 10 | while(1){ |
| ms523 | 2:a51b150e4189 | 11 | // Get the foot positions... |
| ms523 | 2:a51b150e4189 | 12 | pos = FK_Engine(); |
| ms523 | 2:a51b150e4189 | 13 | |
| ms523 | 2:a51b150e4189 | 14 | // Print the results |
| ms523 | 2:a51b150e4189 | 15 | pc.printf("\r F_RHS X = %.0f, Y = %.0f Z = %.0f ",pos.X_fore_right,pos.Y_fore_right,pos.Z_fore_right); |
| ms523 | 2:a51b150e4189 | 16 | pc.printf("F_LHS X = %.0f, Y = %.0f Z = %.0f ",pos.X_fore_left,pos.Y_fore_left,pos.Z_fore_left); |
| ms523 | 2:a51b150e4189 | 17 | pc.printf("H_RHS X = %.0f, Y = %.0f Z = %.0f ",pos.X_hind_right,pos.Y_hind_right,pos.Z_hind_right); |
| ms523 | 2:a51b150e4189 | 18 | pc.printf("H_LHS X = %.0f, Y = %.0f Z = %.0f ",pos.X_hind_left,pos.Y_hind_left,pos.Z_hind_left); |
| ms523 | 2:a51b150e4189 | 19 | |
| ms523 | 2:a51b150e4189 | 20 | // Wait 100 ms |
| ms523 | 2:a51b150e4189 | 21 | wait(0.1); |
| ms523 | 2:a51b150e4189 | 22 | } |
| ms523 | 0:c1e3b0d03727 | 23 | } |