Ticket #5: cwiid-acc_move.diff

File cwiid-acc_move.diff, 10.1 kB (added by wic <martin.wickman@…>, 5 years ago)

Adds integration plugin (see comment in ticket:5

  • cwiid-trunk/libcwiid/command.c

     
    186186        } 
    187187 
    188188        /* Send SET_REPORT */ 
    189         buf[0]=0; 
     189        buf[0]=0x0; 
    190190        buf[1]=rpt_mode; 
    191191        if (send_report(wiimote, 0, RPT_RPT_MODE, RPT_MODE_BUF_LEN, buf)) { 
    192192                cwiid_err(wiimote, "Error setting report state"); 

  • cwiid-trunk/configure.ac

     
    102102        [wminput/plugins/Makefile] 
    103103        [wminput/plugins/ir_ptr/Makefile] 
    104104        [wminput/plugins/acc/Makefile] 
     105        [wminput/plugins/acc_move/Makefile] 
    105106        [wminput/plugins/nunchuk_acc/Makefile] 
    106107        [lswm/Makefile] 
    107108        ) 

  • cwiid-trunk/wminput/plugins/Makefile.in

     
    22 
    33include @top_builddir@/defs.mak 
    44 
    5 PLUGINS = ir_ptr acc nunchuk_acc 
     5PLUGINS = ir_ptr acc nunchuk_acc acc_move 
    66 
    77all install clean distclean uninstall: TARGET += $(MAKECMDGOALS) 
    88 

  • cwiid-trunk/wminput/plugins/acc_move/Makefile.in

     
     1#Copyright (C) 2007 L. Donnie Smith 
     2 
     3include @top_builddir@/defs.mak 
     4 
     5PLUGIN_NAME = acc_move 
     6SOURCES = acc_move.c 
     7CFLAGS += -I@top_builddir@/wminput -I@top_builddir@/libcwiid 
     8LDLIBS += -lm 
     9INST_DIR = $(CWIID_PLUGINS_DIR) 
     10 
     11include $(COMMON)/include/plugin.mak 
     12 
     13distclean: clean 
     14        rm Makefile 
     15 
     16.PHONY: distclean 

  • cwiid-trunk/wminput/plugins/acc_move/acc_move.c

     
     1/* Copyright (C) 2007 L. Donnie Smith <cwiid@abstrakraft.org> 
     2 * 
     3 *  This program is free software; you can redistribute it and/or modify 
     4 *  it under the terms of the GNU General Public License as published by 
     5 *  the Free Software Foundation; either version 2 of the License, or 
     6 *  (at your option) any later version. 
     7 * 
     8 *  This program is distributed in the hope that it will be useful, 
     9 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
     10 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
     11 *  GNU General Public License for more details. 
     12 * 
     13 *  You should have received a copy of the GNU General Public License 
     14 *  along with this program; if not, write to the Free Software 
     15 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA 
     16 * 
     17 *  ChangeLog: 
     18 *  2007-04-15 Martin Wickman <martin.wickman@gmail.com> 
     19 *  * Uses timestep integration for position/velocity 
     20 * 
     21 *  2007-03-01 L. Donnie Smith <cwiid@abstrakraft.org> 
     22 *  * Initial ChangeLog 
     23 *  * made global variables static 
     24 */ 
     25 
     26#include <sys/time.h> 
     27#include <math.h> 
     28 
     29#include "wmplugin.h" 
     30 
     31 
     32struct vec3 { 
     33        double x; 
     34        double y; 
     35        double z; 
     36}; 
     37 
     38#define INTEGRATE_RECT 1 
     39#define INTEGRATE_TRAP 2 
     40 
     41static unsigned char info_init = 0; 
     42static struct wmplugin_info info; 
     43static struct wmplugin_data data; 
     44 
     45static struct vec3 acc_zero, acc_one; 
     46 
     47static int plugin_id; 
     48 
     49wmplugin_info_t wmplugin_info; 
     50wmplugin_init_t wmplugin_init; 
     51wmplugin_exec_t wmplugin_exec; 
     52static void process_acc(struct cwiid_acc_mesg *mesg); 
     53 
     54struct wmplugin_info *wmplugin_info() { 
     55        if (!info_init) { 
     56                info.button_count = 0; 
     57                info.axis_count = 2; 
     58                info.axis_info[0].name = "X"; 
     59                info.axis_info[0].type = WMPLUGIN_REL; 
     60                info.axis_info[0].max  = 0; 
     61                info.axis_info[0].min  = 0; 
     62                info.axis_info[0].fuzz = 0; 
     63                info.axis_info[0].flat = 0; 
     64                info.axis_info[1].name = "Y"; 
     65                info.axis_info[1].type = WMPLUGIN_REL; 
     66                info.axis_info[1].max  = 0; 
     67                info.axis_info[1].min  = 0; 
     68                info.axis_info[1].fuzz = 0; 
     69                info.axis_info[1].flat = 0; 
     70                info.param_count = 5; 
     71                info.param_info[0].name = "XScale"; 
     72                info.param_info[0].type = WMPLUGIN_PARAM_FLOAT; 
     73                info.param_info[0].value.Float = 1.0; 
     74                info.param_info[1].name = "YScale"; 
     75                info.param_info[1].type = WMPLUGIN_PARAM_FLOAT; 
     76                info.param_info[1].value.Float = 1.0; 
     77                info.param_info[2].name = "IntegrationType"; 
     78                info.param_info[2].type = WMPLUGIN_PARAM_INT; 
     79                info.param_info[2].value.Int = INTEGRATE_RECT; 
     80                info.param_info[3].name = "ZeroCountLimit"; 
     81                info.param_info[3].type = WMPLUGIN_PARAM_INT; 
     82                info.param_info[3].value.Int = 25; 
     83                info.param_info[4].name = "ZeroWindow"; 
     84                info.param_info[4].type = WMPLUGIN_PARAM_INT; 
     85                info.param_info[4].value.Int = 5; 
     86 
     87                info_init = 1; 
     88        } 
     89        return &info; 
     90} 
     91 
     92static struct timeval time_prev; 
     93static struct vec3 a0, a1, v0, v1, p0, p1, zero_count; 
     94static int zero_count_limit, zero_window; 
     95 
     96int wmplugin_init(int id, cwiid_wiimote_t *wiimote) 
     97{ 
     98        unsigned char buf[7]; 
     99 
     100        plugin_id = id; 
     101 
     102        data.buttons = 0; 
     103        data.axes[0].valid = 1; 
     104        data.axes[1].valid = 1; 
     105        if (wmplugin_set_report_mode(id, CWIID_RPT_ACC)) { 
     106                return -1; 
     107        } 
     108 
     109        if (cwiid_read(wiimote, CWIID_RW_EEPROM, 0x16, 7, buf)) { 
     110                wmplugin_err(id, "unable to read wiimote info"); 
     111                return -1; 
     112        } 
     113 
     114        acc_zero.x = buf[0]; 
     115        acc_zero.y = buf[1]; 
     116        acc_zero.z = buf[2]; 
     117        acc_one.x  = buf[4]; 
     118        acc_one.y  = buf[5]; 
     119        acc_one.z  = buf[6]; 
     120 
     121        /* 
     122        // Manually calibrated values for a specific wiimote 
     123        acc_zero.x = 125.75; 
     124        acc_zero.y = 126.2;  
     125        */ 
     126 
     127        gettimeofday(&time_prev, NULL); 
     128 
     129        return 0; 
     130} 
     131 
     132struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg *mesg[]) 
     133{ 
     134        int i; 
     135        struct wmplugin_data *ret = NULL; 
     136 
     137        for (i=0; i < mesg_count; i++) { 
     138                switch (mesg[i]->type) { 
     139                case CWIID_MESG_ACC: 
     140                        process_acc(&mesg[i]->acc_mesg); 
     141                        ret = &data; 
     142                        break; 
     143                default: 
     144                        break; 
     145                } 
     146        } 
     147 
     148        return ret; 
     149} 
     150 
     151static void step_rectangle(float dt)  
     152{ 
     153        /* Zero window */ 
     154        if (a1.x <= zero_window && a1.x >= -zero_window) 
     155                a1.x = 0.0f; 
     156        if (a1.y <= zero_window && a1.y >= -zero_window) 
     157                a1.y = 0.0f; 
     158        if (a1.z <= zero_window && a1.z >= -zero_window) 
     159                a1.z = 0.0f; 
     160 
     161        /* Integrate */ 
     162        v1.x += a1.x * dt; 
     163        v1.y += a1.y * dt; 
     164        v1.z += a1.z * dt; 
     165        p1.x += v1.x * dt; 
     166        p1.y += v1.y * dt; 
     167        p1.z += v1.z * dt; 
     168    
     169        /* Movement-end check: stop movement if acc=0 for some time */ 
     170        zero_count.x = fabs(a1.x) < 0.0001f ? zero_count.x + 1 : 0; 
     171        v1.x = zero_count.x >= zero_count_limit ? 0 : v1.x; 
     172         
     173        zero_count.y = fabs(a1.y) < 0.0001f ? zero_count.y + 1 : 0; 
     174        v1.y = zero_count.y >= zero_count_limit ? 0 : v1.y; 
     175 
     176        zero_count.z = fabs(a1.z) < 0.0001f ? zero_count.z + 1 : 0; 
     177        v1.z = zero_count.z >= zero_count_limit ? 0 : v1.z; 
     178} 
     179 
     180 
     181static void step_trapezium(float dt)  
     182{ 
     183        /* Zero window */ 
     184        if (a1.x <= zero_window && a1.x >= -zero_window) 
     185                a1.x = 0.0f; 
     186        if (a1.y <= zero_window && a1.y >= -zero_window) 
     187                a1.y = 0.0f; 
     188        if (a1.z <= zero_window && a1.z >= -zero_window) 
     189                a1.z = 0.0f; 
     190 
     191        /* Integrate */ 
     192        v1.x = v0.x + a0.x*dt + (a1.x - a0.x) * 0.5f*dt; 
     193        v1.y = v0.y + a0.y*dt + (a1.y - a0.y) * 0.5f*dt; 
     194        v1.z = v0.z + a0.z*dt + (a1.z - a0.z) * 0.5f*dt; 
     195 
     196        p1.x = p0.x + v0.x*dt + (v1.x - v0.x) * 0.5f*dt; 
     197        p1.y = p0.y + v0.y*dt + (v1.y - v0.y) * 0.5f*dt; 
     198        p1.z = p0.z + v0.z*dt + (v1.z - v0.z) * 0.5f*dt; 
     199 
     200 
     201        a0.x = a1.x; 
     202        a0.y = a1.y; 
     203        a0.z = a1.z; 
     204 
     205        v0.x = v1.x; 
     206        v0.y = v1.y; 
     207        v0.z = v1.z; 
     208 
     209        /* Movement-end check: stop movement if acc=0 for some time */ 
     210        zero_count.x = fabs(a1.x) < 0.0001f ? zero_count.x + 1 : 0; 
     211        v1.x = zero_count.x >= zero_count_limit ? 0 : v1.x; 
     212        v0.x = zero_count.x >= zero_count_limit ? 0 : v0.x; 
     213         
     214        zero_count.y = fabs(a1.y) < 0.0001f ? zero_count.y + 1 : 0; 
     215        v1.y = zero_count.y >= zero_count_limit ? 0 : v1.y; 
     216        v0.y = zero_count.y >= zero_count_limit ? 0 : v0.y; 
     217 
     218        zero_count.z = fabs(a1.z) < 0.0001f ? zero_count.z + 1 : 0; 
     219        v1.z = zero_count.z >= zero_count_limit ? 0 : v1.z; 
     220        v0.z = zero_count.z >= zero_count_limit ? 0 : v0.z; 
     221} 
     222 
     223static void step (float dt) 
     224{ 
     225        int type = info.param_info[2].value.Int; 
     226        zero_count_limit = info.param_info[3].value.Int; 
     227        zero_window = info.param_info[4].value.Int; 
     228 
     229        if (type == INTEGRATE_RECT) 
     230                step_rectangle(dt); 
     231        else if (type == INTEGRATE_TRAP) 
     232                step_trapezium(dt); 
     233        else 
     234                step_rectangle(dt); 
     235} 
     236 
     237static void process_acc(struct cwiid_acc_mesg *mesg) 
     238{ 
     239        unsigned int ticks; 
     240        struct timeval now; 
     241        gettimeofday(&now, NULL); 
     242        ticks = (now.tv_sec - time_prev.tv_sec) * 1000 +  
     243                (now.tv_usec - time_prev.tv_usec) / 1000; 
     244        float dt = ticks / 1000.0f; 
     245        gettimeofday(&time_prev, NULL); 
     246 
     247        a1.x = -(((double)mesg->x - acc_zero.x)); 
     248        a1.y = (((double)mesg->y - acc_zero.y)); 
     249        a1.z = (((double)mesg->z - acc_zero.z)); 
     250 
     251        a1.z = 0;  
     252 
     253        const float timestep = 0.010; /* 10ms */ 
     254 
     255        int i = 0; 
     256 
     257        /* Some semi-clever timehandling */ 
     258        if (dt <= timestep) { 
     259                step(dt); 
     260        } else { 
     261                while (dt > timestep) { 
     262                        i++; 
     263                        dt -= timestep; 
     264                        step(timestep); 
     265                } 
     266        } 
     267        /* 
     268        printf ("steps: %d dt: %.5f a: %.2f %.2f %.2f " 
     269                  "v: %.2f %.2f %.2f p: %.2f %.2f %.2f old:%d %d \n", 
     270                  i, dt, a1.x, a1.y, a1.z, v1.x, v1.y, v1.z,  
     271                  p1.x, p1.y, p1.z, data.axes[0].value, data.axes[1].value); 
     272        */ 
     273 
     274        data.axes[0].value = v1.x * info.param_info[0].value.Float; 
     275        data.axes[1].value = v1.y * info.param_info[1].value.Float; 
     276} 
     277 

  • cwiid-trunk/wminput/configs/acc_move

     
     1#acc_move 
     2 
     3include buttons 
     4 
     5Plugin.acc_move.X       = REL_X 
     6Plugin.acc_move.Y       = REL_Y 
     7 
     8Plugin.acc_move.XScale  = 10 
     9Plugin.acc_move.YScale  = 10 
     10Plugin.acc_move.IntegrationType = 1  # 1 for rect, 2 for trapezium 
     11Plugin.acc_move.ZeroCountLimit = 20 
     12Plugin.acc_move.ZeroWindow = 5