miso dengaku
KXP84-2050だかのアレ
KXP84.cpp@0:af16997a2d22, 2012-06-20 (annotated)
- Committer:
- misodengaku
- Date:
- Wed Jun 20 22:38:48 2012 +0000
- Revision:
- 0:af16997a2d22
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| misodengaku | 0:af16997a2d22 | 1 | #include "mbed.h" |
| misodengaku | 0:af16997a2d22 | 2 | #include "KXP84.h" |
| misodengaku | 0:af16997a2d22 | 3 | |
| misodengaku | 0:af16997a2d22 | 4 | void KXP84::init() { |
| misodengaku | 0:af16997a2d22 | 5 | char cmd[1]; |
| misodengaku | 0:af16997a2d22 | 6 | //reset=0; |
| misodengaku | 0:af16997a2d22 | 7 | i2c.frequency(400000); |
| misodengaku | 0:af16997a2d22 | 8 | //CTRL_REGB |
| misodengaku | 0:af16997a2d22 | 9 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 10 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 11 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 12 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 13 | *cmd=0x06; |
| misodengaku | 0:af16997a2d22 | 14 | i2c.write(0x0B,cmd,1); |
| misodengaku | 0:af16997a2d22 | 15 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 16 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 17 | //CTRL_REGC |
| misodengaku | 0:af16997a2d22 | 18 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 19 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 20 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 21 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 22 | *cmd=0x00; |
| misodengaku | 0:af16997a2d22 | 23 | i2c.write(0x0A,cmd,1); |
| misodengaku | 0:af16997a2d22 | 24 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 25 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 26 | //FF_INT |
| misodengaku | 0:af16997a2d22 | 27 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 28 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 29 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 30 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 31 | *cmd=0x14; |
| misodengaku | 0:af16997a2d22 | 32 | i2c.write(0x06,cmd,1); |
| misodengaku | 0:af16997a2d22 | 33 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 34 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 35 | //FF_DELAY |
| misodengaku | 0:af16997a2d22 | 36 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 37 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 38 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 39 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 40 | *cmd=0x14; |
| misodengaku | 0:af16997a2d22 | 41 | i2c.write(0x07,cmd,1); |
| misodengaku | 0:af16997a2d22 | 42 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 43 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 44 | //MOT_INT |
| misodengaku | 0:af16997a2d22 | 45 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 46 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 47 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 48 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 49 | *cmd=0x4D; |
| misodengaku | 0:af16997a2d22 | 50 | i2c.write(0x08,cmd,1); |
| misodengaku | 0:af16997a2d22 | 51 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 52 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 53 | //MOT_DELAYC |
| misodengaku | 0:af16997a2d22 | 54 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 55 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 56 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 57 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 58 | *cmd=0x14; |
| misodengaku | 0:af16997a2d22 | 59 | i2c.write(0x09,cmd,1); |
| misodengaku | 0:af16997a2d22 | 60 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 61 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 62 | } |
| misodengaku | 0:af16997a2d22 | 63 | |
| misodengaku | 0:af16997a2d22 | 64 | void KXP84::get(int *x, int *y, int *z) { |
| misodengaku | 0:af16997a2d22 | 65 | char data[6]; |
| misodengaku | 0:af16997a2d22 | 66 | int dx,dy,dz; |
| misodengaku | 0:af16997a2d22 | 67 | i2c.start(); |
| misodengaku | 0:af16997a2d22 | 68 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 69 | i2c.write(0x30); |
| misodengaku | 0:af16997a2d22 | 70 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 71 | i2c.write(0x00); |
| misodengaku | 0:af16997a2d22 | 72 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 73 | i2c.read(0x30,data,6); |
| misodengaku | 0:af16997a2d22 | 74 | wait_us(1); |
| misodengaku | 0:af16997a2d22 | 75 | i2c.stop(); |
| misodengaku | 0:af16997a2d22 | 76 | |
| misodengaku | 0:af16997a2d22 | 77 | dx=data[0]<<4 | data[1]>>4; |
| misodengaku | 0:af16997a2d22 | 78 | dy=data[2]<<4 | data[3]>>4; |
| misodengaku | 0:af16997a2d22 | 79 | dz=data[4]<<4 | data[5]>>4; |
| misodengaku | 0:af16997a2d22 | 80 | |
| misodengaku | 0:af16997a2d22 | 81 | //if (dx>2048) |
| misodengaku | 0:af16997a2d22 | 82 | // *x=dx-2048; |
| misodengaku | 0:af16997a2d22 | 83 | //else |
| misodengaku | 0:af16997a2d22 | 84 | *x=(int)(( (dx - 2048) * 1.24)+0.5); |
| misodengaku | 0:af16997a2d22 | 85 | |
| misodengaku | 0:af16997a2d22 | 86 | //if (dy>2048) |
| misodengaku | 0:af16997a2d22 | 87 | // *y=dy-2048; |
| misodengaku | 0:af16997a2d22 | 88 | //else |
| misodengaku | 0:af16997a2d22 | 89 | *y=(int)(( (dy - 2048) * 1.24)+0.5); |
| misodengaku | 0:af16997a2d22 | 90 | //if (dz>2048) |
| misodengaku | 0:af16997a2d22 | 91 | // *z=dz-2048; |
| misodengaku | 0:af16997a2d22 | 92 | //else |
| misodengaku | 0:af16997a2d22 | 93 | *z=(int)(( (dz - 2048) * 1.24)+0.5); |
| misodengaku | 0:af16997a2d22 | 94 | } |