LTCD_060719.ISO

LT/ADAM/JOY.CPZ

@@ -0,0 +1,136 @@
+#define JOY_CAL_FILE "::/LT/ADAM/JOY.DTZ"
+#define LTPORT 0x201
+#define CMAX  0x800
+
+public I8 joyv[4],joymax[4],joymin[4],joyrest[4];
+public BoolU4 joyb[4],joy_calibrated=FALSE,joy_being_calibrated=FALSE;
+public I8 joysat=100,joynx=0,joyny=0;	//Normalized
+public I8 joycx=grwidth>>1,joycy=grheight>>1,joycvx=0,joycvy=0;
+public I8 joytx=0,joyty=0;  //Text row & col
+
+void PollJoystick()
+{
+  I8 i, j;
+  I8 mask = 15;
+  I8 scale2=24;
+  BoolU4 old_preempt=Preempt(OFF);
+
+  for (i=0;i<4;i++)
+    joyv[i]=0;
+  OutP(LTPORT, 0);
+  for (i=1; mask && i < CMAX; i++) {
+    j = InP(LTPORT) ^ mask;
+    if (j & 1) { joyv[0] = i; mask = mask ^ 1; }
+    if (j & 2) { joyv[1] = i; mask = mask ^ 2; }
+    if (j & 4) { joyv[2] = i; mask = mask ^ 4; }
+    if (j & 8) { joyv[3] = i; mask = mask ^ 8; }
+  }
+  Preempt(old_preempt);
+  j = InP(LTPORT);
+  joyb[0] = !(j & 0x10);
+  joyb[1] = !(j & 0x20);
+  joyb[2] = !(j & 0x40);
+  joyb[3] = !(j & 0x80);
+
+  if (joy_calibrated) {
+    joynx=joysat*(joyv[0]-joyrest[0])/(joymax[0]-joymin[0]+1);
+    if (AbsI8(joynx)<15)
+      joycvx=0;
+    else
+      joycvx+=joynx-SignI8(joynx)*15;
+    joyny=joysat*(joyv[1]-joyrest[1])/(joymax[1]-joymin[1]+1);
+    if (AbsI8(joyny)<15)
+      joycvy=0;
+    else
+      joycvy+=joyny-SignI8(joyny)*15;
+    joycx+=joycvx/scale2;
+    joycvx=70*joycvx/100;
+    joycy+=joycvy/scale2;
+    joycvy=70*joycvy/100;
+    if (joycx < 0) joycx=0;
+    if (joycx>=grwidth) joycx=grwidth-1;
+    if (joycy < 0) joycy=0;
+    if (joycy>=grheight) joycy=grheight-1;
+    joytx=joycx/FONT_WIDTH;
+    joyty=joycy/FONT_HEIGHT;
+  }
+}
+
+
+public void JoyCalibration()
+{
+  I8 i,x,y;
+  I8 buf[3<<2];
+  I8 old_ip=ip_pointer_type;
+  joy_calibrated=FALSE;
+  ip_pointer_type=IP_JOYSTICK0;
+  GrBitMap *base=GrNew(grbase);
+  base->color=ROP_XOR|GR_WHITE;
+  for (i=0;i<4;i++)
+    joyb[i]=0;
+  coutln "$$CL$$Leave joystick and throttle centered\r\n and press button.";
+  while (!(joyb[0] || joyb[1])) {
+    SwapInNext;
+  }
+  for (i=0;i<4;i++) {
+    joyrest[i]=joyv[i];
+    joymax[i]=joyv[i];
+    joymin[i]=joyv[i];
+  }
+  while (joyb[0] || joyb[1]) {
+    SwapInNext;
+  }
+  coutln "$$CL$$Move all about including throttle and press button.";
+  while (!(joyb[0] || joyb[1])) {
+    x=joyv[0]>>1; y=joyv[1]>>1;
+    if (x<3) x=3;
+    if (y<3) y=3;
+    GrLine3(base,x-3,y-3,0,x+3,y+3,0);
+    GrLine3(base,x+3,y-3,0,x-3,y+3,0);
+    x=joyv[2]>>1; y=joyv[3]>>1;
+    if (x<3) x=3;
+    if (y<3) y=3;
+    GrLine3(base,x-3,y-3,0,x+3,y+3,0);
+    GrLine3(base,x+3,y-3,0,x-3,y+3,0);
+    for (i=0;i<4;i++) {
+      if (joyv[i]>joymax[i]) joymax[i]=joyv[i];
+      if (joyv[i]<joymin[i]) joymin[i]=joyv[i];
+    }
+    SwapInNext;
+  }
+  GrClear;
+  joy_calibrated=TRUE;
+
+  //Save
+  for (i=0;i<4;i++) {
+    buf[i*3+0]=joymin[i];
+    buf[i*3+1]=joymax[i];
+    buf[i*3+2]=joyrest[i];
+  }
+  if (DriveIsWritable(':'))
+    WriteFile(JOY_CAL_FILE,buf,sizeof(I8)*3<<2);
+  ip_pointer_type=old_ip;
+  GrDel(base);
+}
+
+BoolU4 LoadJoystickCalibration()
+{
+  U8 size,i;
+  I8 *ptr,*buf=ReadFile(JOY_CAL_FILE,&size);
+  joy_being_calibrated=TRUE;
+  if (!size)
+    JoyCalibration;
+  else {
+    ptr=buf;
+    for (i=0;i<4;i++) {
+      joymin[i]=ptr[0];
+      joymax[i]=ptr[1];
+      joyrest[i]=ptr[2];
+      ptr=ptr+3*sizeof(I8);
+      joy_calibrated=TRUE;
+    }
+    Free(buf);
+  }
+  joy_being_calibrated=FALSE;
+  PollJoystick;
+}