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;
}
}