Drawing Machine #1

This machine consists of 2 servos controlling an aluminum arm with a pen attached.
I origionally developed this hardware using a DS5000 on a simsick.

Code

 /*--------------------------------------------------------------------servo.c
 *  Test program for drawing machines.
 *
 *  This code is targeted at an 89S4051 and compiles with sdcc.
 *
 *  Author: Donald Delmar Davis,
 *  w/ Suggestions from to Bernard Winter and Dan Micheals
 *
 *  Date: 28apr02
 *
 *  Status: alfa VERY alfa.
 *
 *  Notes: This seems to work farely well.
 */

 #include 
 #define NSERVOS 3
 #define SERVO0	P1.7
 #define SERVO1	P1.6
 #define SERVO2	P1.5
 #define KFREQ   11059
 #define STRIM	- 693
 #define ETRIM	+ 81

 unsigned int DegreesX92delay(unsigned int DegreesX9);
 unsigned char CurrentServo;
 unsigned int SpaceDelay;
 unsigned int Servo0Delay;
 unsigned int Servo1Delay;
 unsigned int Servo2Delay;

 /* grid.h is calculated by joe2.c */
 #include "grid.h"

 /*--------------------------------------------------------------------initservos
 *  Initialize servo values and set up timers.
 *
 */

 void initservos(unsigned char s0, unsigned char s1, unsigned char s2 ){

	CurrentServo=0;
	SpaceDelay=65535-((20-(NSERVOS*2))*KFREQ)/12;
	Servo0Delay=DegreesX92delay(s0);
	Servo1Delay=DegreesX92delay(s1);
	Servo2Delay=DegreesX92delay(s2);
        // assembly here is
	// mostly to avoid having to define SERVO pins in both c and asxxx
	// eventually may rewrite routine in assembly.

    _asm
	clr	SERVO0
	clr	SERVO1
	clr	SERVO2
    _endasm;
	// should set flags individually to allow for other timer settings
	TMOD=0x01; //#%00000001
	TCON=0x10; //#%00010000

	TH0=0xFE;
	TL0=0;

	IE=0x82; //#%10000010
 }
 /*--------------------------------------------------------------------servopulse
  * pulse servos sequentially.
  *
  */
 void servopulse(void) interrupt 1 _naked
 {
    _asm
    	clr	TR0
	push	ACC
	push	PSW
	push	B
	clr	SERVO0
	clr	SERVO1
	clr	SERVO2
	mov	A,_CurrentServo
	cjne	A, #NSERVOS, $0001
	mov	A, #255
	mov 	TL0,_SpaceDelay
	mov 	TH0,_SpaceDelay+1
	sjmp	$0007
 $0001:  jnz	$0002
	mov 	TL0,_Servo0Delay
	mov 	TH0,_Servo0Delay+1
	setb	SERVO0
	sjmp	$0007
 $0002:  cjne	A, #1,	$0003
	mov 	TL0,_Servo1Delay
	mov 	TH0,_Servo1Delay+1
	setb	SERVO1
	sjmp	$0007
 $0003:	mov 	TL0,_Servo2Delay
	mov 	TH0,_Servo2Delay+1
	setb    SERVO2
 $0007:	inc	A
	mov	_CurrentServo,A
	pop	B
	pop	PSW
	pop	ACC
	setb	TR0
	reti

     _endasm;
 }

 time1ms()    /* not really a 1 ms delay with XTAL 11.0592MHz */
 {
    int i;
    for (i = 0; i < 8 ; i++)
    ;
 }

 void delay(unsigned int n)      /* do nothing n*1ms */
 {
    int i;
    for (i=0; i< n ; i++)
    time1ms();

 }

 unsigned int DegreesX92delay(unsigned int DegreesX9){
 //delay(10);
 return 0-(690+(DegreesX9));
 }

 void shoulder(unsigned int DegreesX9){
 Servo0Delay=DegreesX92delay(DegreesX9 STRIM);
 }
 void elbow(unsigned int DegreesX9){
 Servo1Delay=DegreesX92delay(DegreesX9 ETRIM);
 }
 void xit (unsigned char x,unsigned char y,unsigned int dly){
 unsigned int s,e;
 unsigned char ds,de;
 s=angles[x][y][0];
 e=angles[x][y][1];

 shoulder(s);elbow(e);delay(dly);

 ds=(char) s - angles[x+1][y][0];
 de=(char) e - angles[x+1][y][1]; 

 ds=ds/3;
 de=de/3;

 shoulder(s+ds);elbow(e+de);delay(dly);
 shoulder(s);elbow(e);delay(dly);
 shoulder(s+ds);elbow(e+de);delay(dly);
 shoulder(s);elbow(e);delay(dly);
 shoulder(s+ds);elbow(e+de);delay(dly);
 shoulder(s);elbow(e);delay(dly);
 shoulder(s+ds);elbow(e+de);delay(dly);
 shoulder(s);elbow(e);delay(dly);
 shoulder(s+ds);elbow(e+de);delay(dly);
 }

 #if 0
 #define GOTO(X,Y)  shoulder(angles[X][Y][0]); elbow(angles[X][Y][1]); delay(dly);
 void xit (unsigned char x,unsigned char y,unsigned int dly){
 unsigned char s,e,n;
 for (e=0;e<5;e++){
 GOTO(x-1,y);
 GOTO(x-1,y+1);
 GOTO(x,y+2);
 GOTO(x+1,y+2);
 GOTO(x+2,y+1);
 GOTO(x+2,y);
 GOTO(x+1,y-1);
 GOTO(x,y-1);
 GOTO(x-1,y);
 }
 }
 #endif

 /*----------------------------------------------------------------------------main()
  *
  */

 main(){

  unsigned char d,e,x,y,n;

  initservos(angles[1][1][0],angles[1][1][1],90);
  d=0;e=0;
  for (n=0; n<50; n++){
     for(x=0;x<15;x++){
	 if (d==0){
	     for(y=1;y<12;y++){
	     xit(x,y,500);
	     }
	     d=1;
         } else {
	     for(y=11;y>0;y--){
	     xit(x,y,500);
	     }
	     d=0;
         }
     }
   e=0;
   }
 }