32dot x 16dot dot matrix display on char. moving by Accelerometer.

Dependencies:   mbed

加速度センサーを使った、傾斜角センシングデモ

アナログタイプの加速度センサーを使って、32x16 LEDドットマトリクス表示上の文章を任意方向にシフトさせます。
水平を保っている場合は、移動なしとし、装置を傾けた場合には、傾けた方向に文字が移動します。
また、移動速度は、傾斜角に比例します。

プログラムは、LPC11U35を搭載したオリジナルCPUボード用に作成しましたが、mbed対応のボードで動作可能です(ポート名は、使用ボードに合わせて変更してください)
部材入手が障壁になりそうですので、このまま作るより、ちょっと変わったUIとして眺めてください
入力部分に使う加速度センサーは比較的容易に入手できます。面白い装置の製作にいかがでしょうか。

オリジナルCPUボードは「MC11U35」の名前で、スイッチサイエンスさんで販売しています。
mbedに対応したCPUを搭載していますので、クラウドでのプログラム作成とUSBからのプログラム書き込みが可能です。

このデモでの構成。
/media/uploads/morita/image_20140716201155a.jpg

・dotマトリクスのLED表示..この当たりで
・加速度センサー
アナログ出力タイプのKXM52を使用しました。
一応Z軸もつないでますが、X軸とY軸しか使っていません。なので2軸センサーでもOKです。
傾斜角はX軸とY軸の電圧として読みだしますので、アナログ出力タイプならそのまま使えます。I2Cとかですと、要変更です。
・CG-ROM
限定した文字ならプログラム内に入らない事もないですが、SPI接続のCGROMが入手できたので、こちらを使いました。 入手不可の場合は、フリーの漢字フォントを探してください。

表示する文章はmainの199行目からの
const uint16_t demo_str[][17]={
にSJISコードで記入します。

接続、
CN2,CN3の番号はMC11U35のコネクタ番号です。P?_xxはLPC11U35のピン名称です。
Dot マトリクスLEDのCOM線 ・・・ P0_23 ・・・ CN3/12番ピン
Dot マトリクスLEDのCA線 ・・・ P0_16 ・・・ CN3/11番ピン
Dot マトリクスLEDのCB線 ・・・ P1_22 ・・・ CN3/10番ピン
Dot マトリクスLEDのCLK線 ・・・ P1_15 ・・・ CN3/13番ピン
Dot マトリクスLEDのLT線 ・・・ P1_13 ・・・ CN3/8番ピン
加速度センサー X軸信号 ・・・ P0_12 ・・・ CN3/5番ピン
加速度センサー Y軸信号 ・・・ P0_13 ・・・ CN3/6番ピン
加速度センサー Z軸信号 ・・・ P0_14 ・・・ CN3/7番ピン
CG-ROM CS ・・・ P0_11 ・・・ CN3/4番ピン
CG-ROM CLK ・・・ P1_29 ・・・ CN3/3番ピン
CG-ROM DI ・・・ P0_9 ・・・ CN3/1番ピン
CG-ROM DO ・・・ P0_8 ・・・ CN2/22番ピン

Committer:
morita
Date:
Wed Jul 16 11:46:13 2014 +0000
Revision:
0:d841459e9b40
MC11U35 demo program V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
morita 0:d841459e9b40 1 #include "mbed.h"
morita 0:d841459e9b40 2
morita 0:d841459e9b40 3 DigitalOut BD7851_COM(P0_23); //32x16 LED display Y line shift data
morita 0:d841459e9b40 4 DigitalOut BD7851_CA(P0_16); // : X line shift data A
morita 0:d841459e9b40 5 DigitalOut BD7851_CB(P1_22); // : X line shift data B
morita 0:d841459e9b40 6 DigitalOut BD7851_CLK(P1_15); // : shift clock
morita 0:d841459e9b40 7 DigitalOut BD7851_LT(P1_13); // : latch
morita 0:d841459e9b40 8 AnalogIn G_X(P0_12); //X axis sensor (Analog)
morita 0:d841459e9b40 9 AnalogIn G_Y(P0_13); //Y axis sensor (Analog)
morita 0:d841459e9b40 10 AnalogIn G_Z(P0_14); //Z axis sensor (Analog)
morita 0:d841459e9b40 11 DigitalOut ROM_CS(P0_11); //CG ROM SPI CS
morita 0:d841459e9b40 12 DigitalOut ROM_CLK(P1_29); //CG ROM SPI clock
morita 0:d841459e9b40 13 DigitalOut ROM_DI(P0_9); //CG ROM SPI MOSI
morita 0:d841459e9b40 14 DigitalIn ROM_DO(P0_8); //CG ROM SPI MISO
morita 0:d841459e9b40 15 DigitalOut LED_R(P0_20); //check LED
morita 0:d841459e9b40 16 DigitalOut LED_I(P0_21); //check LED
morita 0:d841459e9b40 17 DigitalIn ISP(P0_1,PullUp); //Sensor zero adj button
morita 0:d841459e9b40 18
morita 0:d841459e9b40 19 Ticker ticker;
morita 0:d841459e9b40 20 Timer timer;
morita 0:d841459e9b40 21
morita 0:d841459e9b40 22 union _buf{
morita 0:d841459e9b40 23 uint32_t i32;
morita 0:d841459e9b40 24 uint16_t i16[2];
morita 0:d841459e9b40 25 uint8_t i8[4];
morita 0:d841459e9b40 26 };
morita 0:d841459e9b40 27
morita 0:d841459e9b40 28 typedef union _buf flame_buf;
morita 0:d841459e9b40 29
morita 0:d841459e9b40 30 static flame_buf buf[16],bufa[16];
morita 0:d841459e9b40 31 volatile bool copy_sw=false;
morita 0:d841459e9b40 32
morita 0:d841459e9b40 33 //Timer ISR
morita 0:d841459e9b40 34 void disp(void){
morita 0:d841459e9b40 35 static uint8_t pos=0;
morita 0:d841459e9b40 36 static uint16_t ptn,ssr_A=0,ssr_B=0,ssr_COM=0;
morita 0:d841459e9b40 37 uint8_t i;
morita 0:d841459e9b40 38
morita 0:d841459e9b40 39 if(pos==0)ptn=0x8000;
morita 0:d841459e9b40 40 ssr_A=buf[pos].i16[0]; ssr_B=buf[pos].i16[1]; ssr_COM=ptn;
morita 0:d841459e9b40 41 for(i=0;i<16;i++){
morita 0:d841459e9b40 42 BD7851_CA=(ssr_A & 0x8000)==0x8000;
morita 0:d841459e9b40 43 BD7851_CB=(ssr_B & 0x8000)==0x8000;
morita 0:d841459e9b40 44 BD7851_COM=ssr_COM & 1;
morita 0:d841459e9b40 45 ssr_COM >>= 1;
morita 0:d841459e9b40 46 BD7851_CLK=1;
morita 0:d841459e9b40 47 ssr_A <<= 1;
morita 0:d841459e9b40 48 BD7851_CLK=0;
morita 0:d841459e9b40 49 ssr_B <<= 1;
morita 0:d841459e9b40 50 }
morita 0:d841459e9b40 51 BD7851_LT=1;
morita 0:d841459e9b40 52 ptn >>= 1;
morita 0:d841459e9b40 53 BD7851_LT=0;
morita 0:d841459e9b40 54 pos = (pos+1) & 15;
morita 0:d841459e9b40 55 if((pos==0) && (copy_sw)){
morita 0:d841459e9b40 56 copy_sw=false;
morita 0:d841459e9b40 57 memcpy(buf,bufa,4*16);//4byte 16line
morita 0:d841459e9b40 58 }
morita 0:d841459e9b40 59 }
morita 0:d841459e9b40 60
morita 0:d841459e9b40 61 const uint8_t MiniCG[17][8]={
morita 0:d841459e9b40 62 {0x0e,0x11,0x11,0x11,0x11,0x11,0x0e,0x00},
morita 0:d841459e9b40 63 {0x04,0x0c,0x04,0x04,0x04,0x04,0x0e,0x00},
morita 0:d841459e9b40 64 {0x0e,0x11,0x01,0x06,0x08,0x10,0x1f,0x00},
morita 0:d841459e9b40 65 {0x0e,0x11,0x01,0x06,0x01,0x11,0x0e,0x00},
morita 0:d841459e9b40 66 {0x02,0x12,0x12,0x12,0x1f,0x02,0x02,0x00},
morita 0:d841459e9b40 67 {0x1f,0x10,0x10,0x0e,0x01,0x01,0x1e,0x00},
morita 0:d841459e9b40 68 {0x0e,0x10,0x10,0x1e,0x11,0x11,0x0e,0x00},
morita 0:d841459e9b40 69 {0x1f,0x01,0x01,0x02,0x01,0x04,0x04,0x00},
morita 0:d841459e9b40 70 {0x0e,0x11,0x11,0x0e,0x11,0x11,0x0e,0x00},
morita 0:d841459e9b40 71 {0x0e,0x11,0x11,0x0f,0x01,0x01,0x0e,0x00},
morita 0:d841459e9b40 72 {0x04,0x0a,0x11,0x11,0x1f,0x11,0x11,0x00},
morita 0:d841459e9b40 73 {0x1e,0x11,0x11,0x1e,0x11,0x11,0x1e,0x00},
morita 0:d841459e9b40 74 {0x0e,0x11,0x10,0x10,0x10,0x11,0x0e,0x00},
morita 0:d841459e9b40 75 {0x1e,0x11,0x11,0x11,0x11,0x11,0x1e,0x00},
morita 0:d841459e9b40 76 {0x1f,0x10,0x10,0x1e,0x10,0x10,0x1f,0x00},
morita 0:d841459e9b40 77 {0x1f,0x10,0x10,0x1e,0x10,0x10,0x10,0x00},
morita 0:d841459e9b40 78 {0,0,0,0,0,0,0,0}
morita 0:d841459e9b40 79 };
morita 0:d841459e9b40 80
morita 0:d841459e9b40 81 static uint8_t xp=0,yp=0;
morita 0:d841459e9b40 82
morita 0:d841459e9b40 83 void put_c(uint8_t c){
morita 0:d841459e9b40 84 uint8_t i;
morita 0:d841459e9b40 85
morita 0:d841459e9b40 86 for(i=0;i<8;i++){
morita 0:d841459e9b40 87 buf[i+(yp<<3)].i32=buf[i+(yp<<3)].i32 | (MiniCG[c][i])<<((4-xp)*6)+2;
morita 0:d841459e9b40 88 }
morita 0:d841459e9b40 89 if(++xp>4){
morita 0:d841459e9b40 90 xp=0;
morita 0:d841459e9b40 91 yp++;
morita 0:d841459e9b40 92 if(yp>1){
morita 0:d841459e9b40 93 yp=1;
morita 0:d841459e9b40 94 for(i=0;i<8;i++){
morita 0:d841459e9b40 95 buf[i].i32=buf[i+8].i32;
morita 0:d841459e9b40 96 buf[i+8].i32=0;
morita 0:d841459e9b40 97 }
morita 0:d841459e9b40 98 }
morita 0:d841459e9b40 99 }
morita 0:d841459e9b40 100 }
morita 0:d841459e9b40 101
morita 0:d841459e9b40 102 void put_2H(uint8_t d){
morita 0:d841459e9b40 103 put_c(d >> 4);
morita 0:d841459e9b40 104 put_c(d & 15);
morita 0:d841459e9b40 105 }
morita 0:d841459e9b40 106 void put_4H(uint16_t dd){
morita 0:d841459e9b40 107 put_2H(dd >> 8);
morita 0:d841459e9b40 108 put_2H(dd & 255);
morita 0:d841459e9b40 109 }
morita 0:d841459e9b40 110
morita 0:d841459e9b40 111 void ROM_ssr8(uint8_t ssr){
morita 0:d841459e9b40 112 uint8_t i;
morita 0:d841459e9b40 113
morita 0:d841459e9b40 114 for(i=0;i<8;i++){
morita 0:d841459e9b40 115 if(ssr & 128)ROM_DI=1; else ROM_DI=0;
morita 0:d841459e9b40 116 ssr <<= 1;
morita 0:d841459e9b40 117 ROM_CLK=1;
morita 0:d841459e9b40 118 // wait_us(1);
morita 0:d841459e9b40 119 ROM_CLK=0;
morita 0:d841459e9b40 120 }
morita 0:d841459e9b40 121 }
morita 0:d841459e9b40 122
morita 0:d841459e9b40 123 //read one charter (32bytes) SJIS code
morita 0:d841459e9b40 124 void read_block_CG(uint16_t code,uint8_t *cgb){
morita 0:d841459e9b40 125 uint8_t i,j,ssr;
morita 0:d841459e9b40 126 unsigned char c1, c2, MSB ,LSB;
morita 0:d841459e9b40 127 uint32_t address, seq;
morita 0:d841459e9b40 128
morita 0:d841459e9b40 129 // SJIS to kuten code conversion
morita 0:d841459e9b40 130 c1 = (code>>8);
morita 0:d841459e9b40 131 c2 = (code & 0xFF);
morita 0:d841459e9b40 132 seq = (c1<=159 ? c1-129 : c1-193)*188 + (c2<=126 ? c2-64 : c2-65);
morita 0:d841459e9b40 133 MSB = seq / 94 + 1;
morita 0:d841459e9b40 134 LSB = seq % 94 + 1;
morita 0:d841459e9b40 135 address = 0;
morita 0:d841459e9b40 136
morita 0:d841459e9b40 137 if( MSB >= 1 && MSB <= 15 && LSB >= 1 && LSB <= 94)
morita 0:d841459e9b40 138 address =( (MSB - 1) * 94 + (LSB - 1))*32;
morita 0:d841459e9b40 139 else if(MSB >= 16 && MSB <= 47 && LSB >= 1 && LSB <= 94)
morita 0:d841459e9b40 140 address =( (MSB - 16) * 94 + (LSB - 1))*32 + 0x0AA40L;
morita 0:d841459e9b40 141 else if(MSB >= 48 && MSB <= 84 && LSB >= 1 && LSB <= 94)
morita 0:d841459e9b40 142 address = ((MSB - 48) * 94 + (LSB - 1))*32 + 0x21CDFL;
morita 0:d841459e9b40 143 else if(MSB == 85 && LSB >= 1 && LSB <= 94)
morita 0:d841459e9b40 144 address = ((MSB - 85) * 94 + (LSB - 1))*32 + 0x3C4A0L;
morita 0:d841459e9b40 145 else if(MSB >= 88 && MSB <= 89 && LSB >= 1 && LSB <= 94)
morita 0:d841459e9b40 146 address = ((MSB - 88) * 94 + (LSB - 1))*32 + 0x3D060L;
morita 0:d841459e9b40 147
morita 0:d841459e9b40 148 ROM_CS=0;
morita 0:d841459e9b40 149 ROM_CLK=0;
morita 0:d841459e9b40 150 ROM_ssr8(0x03);//read inst.
morita 0:d841459e9b40 151 ROM_ssr8((address >> 16) & 0xff);//read address A23..A16
morita 0:d841459e9b40 152 ROM_ssr8((address >> 8) & 0xff);//read address A15..A8
morita 0:d841459e9b40 153 ROM_ssr8(address & 0xff);//read address A7..A0
morita 0:d841459e9b40 154
morita 0:d841459e9b40 155 for(j=0;j<32;j++){
morita 0:d841459e9b40 156 ssr=0;
morita 0:d841459e9b40 157 for(i=0;i<8;i++){
morita 0:d841459e9b40 158 ssr <<= 1;
morita 0:d841459e9b40 159 ROM_CLK=1;
morita 0:d841459e9b40 160 if(ROM_DO != 0)ssr |= 1;
morita 0:d841459e9b40 161 ROM_CLK=0;
morita 0:d841459e9b40 162 }
morita 0:d841459e9b40 163 *cgb++=ssr;
morita 0:d841459e9b40 164 }
morita 0:d841459e9b40 165 ROM_CS=1;
morita 0:d841459e9b40 166 }
morita 0:d841459e9b40 167
morita 0:d841459e9b40 168 void put_cgp(uint16_t code,uint16_t *pbuf){
morita 0:d841459e9b40 169 uint8_t i,j,k[32];
morita 0:d841459e9b40 170 uint16_t tmp;
morita 0:d841459e9b40 171
morita 0:d841459e9b40 172 read_block_CG(code,k);//get CG (32 bytes)
morita 0:d841459e9b40 173
morita 0:d841459e9b40 174 for(i=0;i<16;i++)pbuf[i]=0;
morita 0:d841459e9b40 175
morita 0:d841459e9b40 176 for(i=0;i<8;i++){
morita 0:d841459e9b40 177 for(j=0;j<8;j++){
morita 0:d841459e9b40 178 pbuf[j] <<= 1;
morita 0:d841459e9b40 179 pbuf[j+8] <<= 1;
morita 0:d841459e9b40 180
morita 0:d841459e9b40 181 tmp=0;
morita 0:d841459e9b40 182 if(k[i+0] & 1) tmp = 0x100;
morita 0:d841459e9b40 183 if(k[i+8] & 1) tmp |= 1;
morita 0:d841459e9b40 184 pbuf[j] |= tmp;
morita 0:d841459e9b40 185
morita 0:d841459e9b40 186 tmp=0;
morita 0:d841459e9b40 187 if(k[i+16] & 1) tmp = 0x100;
morita 0:d841459e9b40 188 if(k[i+24] & 1) tmp |= 1;
morita 0:d841459e9b40 189 pbuf[j+8] |= tmp;
morita 0:d841459e9b40 190
morita 0:d841459e9b40 191 k[i+0] >>= 1;
morita 0:d841459e9b40 192 k[i+8] >>= 1;
morita 0:d841459e9b40 193 k[i+16] >>= 1;
morita 0:d841459e9b40 194 k[i+24] >>= 1;
morita 0:d841459e9b40 195 }
morita 0:d841459e9b40 196 }
morita 0:d841459e9b40 197 }
morita 0:d841459e9b40 198
morita 0:d841459e9b40 199 const uint16_t demo_str[][17]={
morita 0:d841459e9b40 200 {0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140},
morita 0:d841459e9b40 201 {0x8140,0x828D,0x8282,0x8285,0x8284,0x834E,0x8389,0x8345,0x8368,0x82CD,0x95D6,0x9798,0x82C5,0x82B7,0x8142,0x8140,0x8140},
morita 0:d841459e9b40 202 {0x8140,0x8352,0x8393,0x8370,0x8343,0x8389,0x82CC,0x8343,0x8393,0x8358,0x8367,0x815B,0x838B,0x82C6,0x82A9,0x8141,0x8140},
morita 0:d841459e9b40 203 {0x8140,0x8AC2,0x8BAB,0x90DD,0x92E8,0x82C6,0x82A9,0x8141,0x8E67,0x82A2,0x8E6E,0x82DF,0x82DC,0x82C5,0x82C9,0x954B,0x8140},
morita 0:d841459e9b40 204 {0x8140,0x9776,0x82C8,0x8DEC,0x8BC6,0x82CD,0x9573,0x9776,0x82C5,0x82B7,0x8142,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140},
morita 0:d841459e9b40 205 {0x8140,0x8352,0x8393,0x8370,0x8343,0x838B,0x82AA,0x8F49,0x82ED,0x82EA,0x82CE,0x8141,0x8274,0x8272,0x8261,0x8381,0x8140},
morita 0:d841459e9b40 206 {0x8140,0x8382,0x838A,0x82C9,0x836F,0x8343,0x8369,0x838A,0x82F0,0x935D,0x9197,0x82B7,0x82E9,0x82BE,0x82AF,0x8142,0x8140},
morita 0:d841459e9b40 207 {0x8140,0x8F91,0x82AB,0x8D9E,0x82DD,0x9180,0x8DEC,0x82F0,0x82B5,0x82C4,0x82A2,0x82E9,0x8AB4,0x8A6F,0x82B7,0x82E7,0x8140},
morita 0:d841459e9b40 208 {0x8140,0x82A0,0x82E8,0x82DC,0x82B9,0x82F1,0x8142,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140},
morita 0:d841459e9b40 209 {0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140,0x8140}
morita 0:d841459e9b40 210 };
morita 0:d841459e9b40 211 //messeage size
morita 0:d841459e9b40 212 #define X_MAX ((17-2)*16-1)
morita 0:d841459e9b40 213 #define Y_MAX ((10-3)*16)
morita 0:d841459e9b40 214
morita 0:d841459e9b40 215 int main() {
morita 0:d841459e9b40 216 uint8_t i,j,k,xp,yp;
morita 0:d841459e9b40 217 uint16_t wbuf[3][4][16];
morita 0:d841459e9b40 218 uint16_t gx,gy;
morita 0:d841459e9b40 219 int16_t x=0,y=0;
morita 0:d841459e9b40 220 int32_t dx=1,dy=1;
morita 0:d841459e9b40 221 uint8_t tmx=1,tmy=1;
morita 0:d841459e9b40 222 int16_t tmxv=1,tmyv=1;
morita 0:d841459e9b40 223 uint8_t ofx,ofy;
morita 0:d841459e9b40 224 uint64_t cl;
morita 0:d841459e9b40 225 uint16_t AD_offset_X=32768,AD_offset_Y=32768;
morita 0:d841459e9b40 226
morita 0:d841459e9b40 227 ROM_CS=1;
morita 0:d841459e9b40 228 ticker.attach_us(&disp, 200);//int=5KHz frame rate=300Hz
morita 0:d841459e9b40 229
morita 0:d841459e9b40 230 for(k=0;k<3;k++){
morita 0:d841459e9b40 231 for(j=0;j<4;j++){
morita 0:d841459e9b40 232 for(i=0;i<16;i++){
morita 0:d841459e9b40 233 wbuf[k][j][i]=0;
morita 0:d841459e9b40 234 }
morita 0:d841459e9b40 235 }
morita 0:d841459e9b40 236 }
morita 0:d841459e9b40 237 /*
morita 0:d841459e9b40 238 for(i=0;i<4;i++)put_2H(moji[i]);
morita 0:d841459e9b40 239 while(1);
morita 0:d841459e9b40 240 */
morita 0:d841459e9b40 241
morita 0:d841459e9b40 242 timer.start();
morita 0:d841459e9b40 243 while(1){
morita 0:d841459e9b40 244 xp=(x+0)>>4; yp=(y+16)>>4;
morita 0:d841459e9b40 245 ofx=x & 15; ofy=y & 15;
morita 0:d841459e9b40 246 put_cgp(demo_str[yp][xp],wbuf[1][1]);//1'st
morita 0:d841459e9b40 247 put_cgp(demo_str[yp][xp+1],wbuf[1][2]);//2'nd
morita 0:d841459e9b40 248 put_cgp(demo_str[yp][xp+2],wbuf[1][3]);//right
morita 0:d841459e9b40 249 put_cgp(demo_str[yp+1][xp],wbuf[2][1]);//botom 1'st
morita 0:d841459e9b40 250 put_cgp(demo_str[yp+1][xp+1],wbuf[2][2]);//botom 2'nd
morita 0:d841459e9b40 251 put_cgp(demo_str[yp+1][xp+2],wbuf[2][3]);//botom right
morita 0:d841459e9b40 252 if(copy_sw==false){
morita 0:d841459e9b40 253 for(j=0;j<16;j++){
morita 0:d841459e9b40 254 i=j+ofy;
morita 0:d841459e9b40 255 cl=((uint64_t)wbuf[(i>>4)+1][1][i & 15]<<32)|((uint64_t)wbuf[(i>>4)+1][2][i & 15]<<16)|(uint64_t)wbuf[(i>>4)+1][3][i & 15];
morita 0:d841459e9b40 256 cl >>= (16-ofx);
morita 0:d841459e9b40 257 bufa[j].i32=cl;
morita 0:d841459e9b40 258 }
morita 0:d841459e9b40 259 copy_sw=true;//
morita 0:d841459e9b40 260 }
morita 0:d841459e9b40 261 if(timer.read_ms()>3){
morita 0:d841459e9b40 262 timer.reset();
morita 0:d841459e9b40 263
morita 0:d841459e9b40 264 gx=G_X.read_u16();
morita 0:d841459e9b40 265 if(ISP==0)AD_offset_X=gx;
morita 0:d841459e9b40 266 dx=dx+(gx-AD_offset_X)/50;
morita 0:d841459e9b40 267 if(dx>999)dx=999;
morita 0:d841459e9b40 268 if(dx<-999)dx=-999;
morita 0:d841459e9b40 269 dx=dx*9000/10000;
morita 0:d841459e9b40 270 if(dx)dx--;
morita 0:d841459e9b40 271 if(dx<0)dx++;
morita 0:d841459e9b40 272
morita 0:d841459e9b40 273 gy=G_Y.read_u16();
morita 0:d841459e9b40 274 if(ISP==0)AD_offset_Y=gy;
morita 0:d841459e9b40 275 dy=dy-(gy-AD_offset_Y)/50;
morita 0:d841459e9b40 276 if(dy>999)dy=999;
morita 0:d841459e9b40 277 if(dy<-999)dy=-999;
morita 0:d841459e9b40 278 dy=dy*9000/10000;
morita 0:d841459e9b40 279 if(dy)dy--;
morita 0:d841459e9b40 280 if(dy<0)dy++;
morita 0:d841459e9b40 281 //put_4H(dx);
morita 0:d841459e9b40 282 //put_c(16);//blank
morita 0:d841459e9b40 283 if(dx != 0){
morita 0:d841459e9b40 284 tmxv=1000/dx;
morita 0:d841459e9b40 285 if(tmxv<0)tmxv = -tmxv;
morita 0:d841459e9b40 286 if(tmxv>100)tmxv=100;
morita 0:d841459e9b40 287 if(tmx>tmxv)tmx=tmxv;
morita 0:d841459e9b40 288 if(--tmx==0){
morita 0:d841459e9b40 289 tmx=tmxv;
morita 0:d841459e9b40 290 LED_I = LED_I ^ 1;
morita 0:d841459e9b40 291 if(dx>0){
morita 0:d841459e9b40 292 x=x+1;
morita 0:d841459e9b40 293 if(x>X_MAX){
morita 0:d841459e9b40 294 x=X_MAX;
morita 0:d841459e9b40 295 }
morita 0:d841459e9b40 296 }
morita 0:d841459e9b40 297 if(dx<0){
morita 0:d841459e9b40 298 x=x-1;
morita 0:d841459e9b40 299 if(x<0){
morita 0:d841459e9b40 300 x=0;
morita 0:d841459e9b40 301 }
morita 0:d841459e9b40 302 }
morita 0:d841459e9b40 303 }
morita 0:d841459e9b40 304 } else {
morita 0:d841459e9b40 305 LED_I=0;
morita 0:d841459e9b40 306 }
morita 0:d841459e9b40 307 if(dy != 0){
morita 0:d841459e9b40 308 tmyv=1000/dy;
morita 0:d841459e9b40 309 if(tmyv<0)tmyv = -tmyv;
morita 0:d841459e9b40 310 if(tmyv>100)tmyv=100;
morita 0:d841459e9b40 311 if(tmy>tmyv)tmy=tmyv;
morita 0:d841459e9b40 312 if(--tmy==0){
morita 0:d841459e9b40 313 LED_R=LED_R ^ 1;
morita 0:d841459e9b40 314 tmy=tmyv;
morita 0:d841459e9b40 315 if(dy>0){
morita 0:d841459e9b40 316 y=y+1;
morita 0:d841459e9b40 317 if(y>Y_MAX){
morita 0:d841459e9b40 318 y=Y_MAX;
morita 0:d841459e9b40 319 }
morita 0:d841459e9b40 320 }
morita 0:d841459e9b40 321 if(dy<0){
morita 0:d841459e9b40 322 y=y-1;
morita 0:d841459e9b40 323 if(y<0){
morita 0:d841459e9b40 324 y=0;
morita 0:d841459e9b40 325 }
morita 0:d841459e9b40 326 }
morita 0:d841459e9b40 327 }
morita 0:d841459e9b40 328 } else {
morita 0:d841459e9b40 329 LED_R=0;
morita 0:d841459e9b40 330 }
morita 0:d841459e9b40 331 }
morita 0:d841459e9b40 332 }
morita 0:d841459e9b40 333 }