abstractions to handle rotatio as reported by motionplus

svn path=/trunk/externals/hardware/wiimote/; revision=12547

2 files changed, 322 insertions, 0 deletions

diff --git a/motionplus.pd b/motionplus.pd
new file mode 100644
index 0000000..4619dd6
--- /dev/null
+++ b/motionplus.pd
@@ -0,0 +1,202 @@
+#N canvas 325 242 656 565 10;
+#X obj 51 48 inlet wiimotionplus;
+#X obj 51 70 route wiimotionplus;
+#X text 196 39 just in case somebody uses the [wiimote]s output directly
+;
+#N canvas 676 229 450 300 slowfast 0;
+#X obj 105 55 inlet;
+#X obj 105 77 route angle_rate low_speed;
+#X obj 105 107 unpack 0 0 0;
+#X obj 194 107 unpack 0 0 0;
+#X obj 105 232 pack;
+#X obj 105 254 outlet pitch;
+#X obj 195 232 pack;
+#X obj 278 232 pack;
+#X obj 195 254 outlet roll;
+#X obj 278 254 outlet yaw;
+#X connect 0 0 1 0;
+#X connect 1 0 2 0;
+#X connect 1 1 3 0;
+#X connect 2 0 4 0;
+#X connect 2 1 6 0;
+#X connect 2 2 7 0;
+#X connect 3 0 4 1;
+#X connect 3 1 6 1;
+#X connect 3 2 7 1;
+#X connect 4 0 5 0;
+#X connect 6 0 8 0;
+#X connect 7 0 9 0;
+#X restore 51 98 pd slowfast;
+#X obj 51 297 pack 0 0 0;
+#X obj 51 120 motionplus_rate2angle;
+#X obj 51 319 outlet yaw roll pitch;
+#X obj 218 230 pack 0 0 0;
+#X obj 242 118 t a a a;
+#X msg 242 85 reset;
+#X msg 300 95 deadzone \$1;
+#X floatatom 300 75 5 0 0 0 - - -;
+#X obj 218 322 outlet dYaw dRoll dPitch;
+#X obj 88 150 motionplus_rate2angle;
+#X obj 125 180 motionplus_rate2angle;
+#N canvas 0 0 450 300 angleaxis 1;
+#X obj 142 209 mtx_*;
+#X obj 142 231 t a a;
+#X obj 273 154 loadbang;
+#X msg 142 155 \$1 0 0;
+#N canvas 295 417 569 550 angleaxis2mtx 0;
+#X obj 73 43 inlet dYaw dRoll dPitch;
+#N canvas 0 0 450 300 length 0;
+#X obj 84 59 t f f;
+#X obj 84 81 *;
+#X obj 84 103 +;
+#X obj 84 125 outlet;
+#X obj 134 59 t f f;
+#X obj 134 81 *;
+#X obj 134 103 +;
+#X obj 184 59 t f f;
+#X obj 184 81 *;
+#X obj 84 37 unpack 0 0 0;
+#X obj 84 15 inlet;
+#X connect 0 0 1 0;
+#X connect 0 1 1 1;
+#X connect 1 0 2 0;
+#X connect 2 0 3 0;
+#X connect 4 0 5 0;
+#X connect 4 1 5 1;
+#X connect 5 0 6 0;
+#X connect 6 0 2 1;
+#X connect 7 0 8 0;
+#X connect 7 1 8 1;
+#X connect 8 0 6 1;
+#X connect 9 0 0 0;
+#X connect 9 1 4 0;
+#X connect 9 2 7 0;
+#X connect 10 0 9 0;
+#X restore 105 84 pd length;
+#X obj 73 65 t l l;
+#N canvas 0 0 450 300 normalize 0;
+#X obj 74 36 inlet;
+#X obj 74 58 unpack 0 0 0;
+#X obj 74 102 pack 0 0 0;
+#X obj 74 124 outlet;
+#X obj 74 80 * 1;
+#X obj 114 80 * 1;
+#X obj 155 80 * 1;
+#X obj 261 33 inlet length;
+#X msg 261 55 1 \$1;
+#X obj 261 77 /;
+#X obj 165 57 t f f f;
+#X connect 0 0 1 0;
+#X connect 1 0 4 0;
+#X connect 1 1 5 0;
+#X connect 1 2 6 0;
+#X connect 2 0 3 0;
+#X connect 4 0 2 0;
+#X connect 5 0 2 1;
+#X connect 6 0 2 2;
+#X connect 7 0 8 0;
+#X connect 8 0 9 0;
+#X connect 9 0 10 0;
+#X connect 10 0 4 1;
+#X connect 10 1 5 1;
+#X connect 10 2 6 1;
+#X restore 73 145 pd normalize;
+#X obj 105 106 t f f;
+#N canvas 0 0 450 300 deg2rad 0;
+#X obj 109 112 inlet deg;
+#X obj 109 231 outlet rad;
+#X obj 181 179 / 45;
+#X obj 181 113 loadbang;
+#X obj 109 202 * 0.0174533;
+#X obj 109 178 f;
+#X text 93 282 rad=deg*pi/180;
+#X obj 181 156 atan;
+#X msg 181 135 1;
+#X text 225 36 part of zexy;
+#X text 63 66 convert DEGree to RADiant;
+#X connect 0 0 5 0;
+#X connect 2 0 4 1;
+#X connect 3 0 8 0;
+#X connect 4 0 1 0;
+#X connect 5 0 4 0;
+#X connect 7 0 2 0;
+#X connect 8 0 7 0;
+#X restore 171 146 pd deg2rad;
+#X obj 171 168 t f f;
+#X obj 171 190 cos;
+#X obj 171 212 t f f;
+#X obj 203 234 * -1;
+#X obj 203 256 + 1;
+#X obj 203 190 sin;
+#X text 297 81 c=cos() \, s=sin() \, C=1-c;
+#X text 297 141 [ z*x*C-y*s y*z*C+x*s z*z*C+c ];
+#X text 297 100 [ x*x*C+c x*y*C-z*s z*x*C+y*s ];
+#X text 297 121 [ x*y*C+z*s y*y*C+c y*z*C-x*s ];
+#X obj 73 282 pack 0 0 0 0 0 0;
+#X obj 310 255 expr;
+#X obj 73 305 expr $f1*$f1*$f6+$f4 \; $f1*$f2*$f6-$f3*$f5 \; $f1*$f3*$f6+$f2*$f5
+\; $f1*$f2*$f6+$f3*$f5 \; $f2*$f2*$f6+$f4 \; $f2*$f3*$f6-$f1*$f5 \;
+$f1*$f3*$f6-$f2*$f5 \; $f2*$f3*$f6+$f1*$f5 \; $f3*$f3*$f6+$f4;
+#X obj 73 430 pack 0 0 0 0 0 0 0 0 0;
+#X obj 73 452 list prepend matrix 3 3;
+#X obj 73 474 list trim;
+#X obj 73 496 outlet Rot;
+#X connect 0 0 2 0;
+#X connect 1 0 4 0;
+#X connect 2 0 3 0;
+#X connect 2 1 1 0;
+#X connect 3 0 16 0;
+#X connect 4 0 3 1;
+#X connect 4 1 5 0;
+#X connect 5 0 6 0;
+#X connect 6 0 7 0;
+#X connect 6 1 11 0;
+#X connect 7 0 8 0;
+#X connect 8 0 16 3;
+#X connect 8 1 9 0;
+#X connect 9 0 10 0;
+#X connect 10 0 16 5;
+#X connect 11 0 16 4;
+#X connect 16 0 18 0;
+#X connect 18 0 19 0;
+#X connect 18 1 19 1;
+#X connect 18 2 19 2;
+#X connect 18 3 19 3;
+#X connect 18 4 19 4;
+#X connect 18 5 19 5;
+#X connect 18 6 19 6;
+#X connect 18 7 19 7;
+#X connect 18 8 19 8;
+#X connect 19 0 20 0;
+#X connect 20 0 21 0;
+#X connect 21 0 22 0;
+#X restore 142 182 pd angleaxis2mtx;
+#X text 170 22 doesn't really work;
+#X msg 273 181 matrix 3 3 1 0 0 0 1 0 0 0 1;
+#X connect 0 0 1 0;
+#X connect 1 1 0 1;
+#X connect 2 0 6 0;
+#X connect 3 0 4 0;
+#X connect 4 0 0 0;
+#X connect 6 0 0 1;
+#X restore 485 276 pd angleaxis;
+#X connect 0 0 1 0;
+#X connect 1 0 3 0;
+#X connect 1 1 3 0;
+#X connect 3 0 5 0;
+#X connect 3 1 13 0;
+#X connect 3 2 14 0;
+#X connect 4 0 6 0;
+#X connect 5 0 4 0;
+#X connect 5 1 7 0;
+#X connect 7 0 12 0;
+#X connect 8 0 5 1;
+#X connect 8 1 13 1;
+#X connect 8 2 14 1;
+#X connect 9 0 8 0;
+#X connect 10 0 8 0;
+#X connect 11 0 10 0;
+#X connect 13 0 4 1;
+#X connect 13 1 7 1;
+#X connect 14 0 4 2;
+#X connect 14 1 7 2;
diff --git a/motionplus_rate2angle.pd b/motionplus_rate2angle.pd
new file mode 100644
index 0000000..cb4d87d
--- /dev/null
+++ b/motionplus_rate2angle.pd
@@ -0,0 +1,120 @@
+#N canvas 0 0 450 357 10;
+#N canvas 0 0 704 467 anglespeed 0;
+#X obj 149 57 loadbang;
+#X obj 95 108 t l l;
+#X msg 95 85 \$2 \$1;
+#X obj 127 151 route 1;
+#X obj 95 232 unpack;
+#X obj 95 255 pack;
+#X obj 95 277 route 0 1;
+#X obj 95 299 / 4;
+#X obj 95 321 t f;
+#X obj 125 299 / 20;
+#X obj 149 78 t b b;
+#X obj 149 100 f 8063;
+#X obj 134 254 - 8063;
+#X msg 149 122 set \$1;
+#X obj 95 36 inlet <angle_rate> <low_speed>;
+#X obj 95 393 outlet;
+#X obj 127 173 mavg 1000;
+#X obj 346 58 r \$0-reset;
+#X obj 173 223 t f f;
+#X obj 173 201 f 8063;
+#X connect 0 0 10 0;
+#X connect 1 0 4 0;
+#X connect 1 1 3 0;
+#X connect 2 0 1 0;
+#X connect 3 0 16 0;
+#X connect 4 0 5 0;
+#X connect 4 1 12 0;
+#X connect 5 0 6 0;
+#X connect 6 0 7 0;
+#X connect 6 1 9 0;
+#X connect 7 0 8 0;
+#X connect 8 0 15 0;
+#X connect 9 0 8 0;
+#X connect 10 0 11 0;
+#X connect 10 1 19 0;
+#X connect 11 0 13 0;
+#X connect 12 0 5 1;
+#X connect 13 0 16 0;
+#X connect 14 0 2 0;
+#X connect 16 0 19 1;
+#X connect 17 0 10 0;
+#X connect 18 0 12 1;
+#X connect 18 1 11 1;
+#X connect 19 0 18 0;
+#X restore 51 155 pd anglespeed;
+#N canvas 0 0 450 300 deadzone 0;
+#X obj 90 101 t f;
+#X obj 90 123 moses -0.5;
+#X obj 157 144 moses 0.5;
+#X msg 157 166 0;
+#X obj 158 104 * -1;
+#X obj 157 188 t f;
+#X obj 157 210 outlet;
+#X obj 158 82 t f f;
+#X obj 90 46 inlet dphi;
+#X obj 225 45 r \$0-deadzone;
+#X connect 0 0 1 0;
+#X connect 1 0 5 0;
+#X connect 1 1 2 0;
+#X connect 2 0 3 0;
+#X connect 2 1 5 0;
+#X connect 3 0 5 0;
+#X connect 4 0 1 1;
+#X connect 5 0 6 0;
+#X connect 7 0 4 0;
+#X connect 7 1 2 1;
+#X connect 8 0 0 0;
+#X connect 9 0 7 0;
+#X restore 51 202 pd deadzone;
+#N canvas 0 0 450 300 accumulate 0;
+#X obj 95 179 t f;
+#X obj 95 201 +;
+#X obj 95 223 t f f;
+#X msg 143 180 0;
+#X obj 95 156 inlet;
+#X obj 95 245 outlet;
+#X obj 143 161 r \$0-reset;
+#X connect 0 0 1 0;
+#X connect 1 0 2 0;
+#X connect 2 0 5 0;
+#X connect 2 1 1 1;
+#X connect 3 0 1 1;
+#X connect 4 0 0 0;
+#X connect 6 0 3 0;
+#X restore 51 257 pd accumulate;
+#N canvas 0 0 1087 300 pertime 0;
+#X obj 83 53 inlet;
+#X obj 83 75 t f;
+#X obj 83 97 / 100;
+#X obj 83 119 outlet;
+#X text 137 96 <-- currently hardcoded to 10 fps \; LATER make use
+of the timetags in the cwiid message and see whether this helps;
+#X connect 0 0 1 0;
+#X connect 1 0 2 0;
+#X connect 2 0 3 0;
+#X restore 51 177 pd pertime;
+#X obj 268 137 route reset deadzone;
+#X obj 268 159 t b;
+#X obj 268 181 s \$0-reset;
+#X obj 268 115 inlet control;
+#X obj 51 128 inlet angle_rate;
+#X obj 51 279 outlet angle;
+#X obj 336 212 t f;
+#X obj 336 234 s \$0-deadzone;
+#X obj 183 279 outlet dAngle;
+#X obj 51 229 t f f;
+#X connect 0 0 3 0;
+#X connect 1 0 13 0;
+#X connect 2 0 9 0;
+#X connect 3 0 1 0;
+#X connect 4 0 5 0;
+#X connect 4 1 10 0;
+#X connect 5 0 6 0;
+#X connect 7 0 4 0;
+#X connect 8 0 0 0;
+#X connect 10 0 11 0;
+#X connect 13 0 2 0;
+#X connect 13 1 12 0;