w_bwr
Member level 3
- Joined
- Feb 4, 2010
- Messages
- 66
- Helped
- 0
- Reputation
- 0
- Reaction score
- 0
- Trophy points
- 1,286
- Location
- Karachi, Pakistan
- Activity points
- 1,810
Controller : ST10F276
Compiler : Tasking
I have written a program to drive two servos simultaneously. Both Motors are programmed to have identical movement.
Motor 1 is connected to P7.0 & Motor 2 is connected to P7.2. The program works fine with only one motor regardless of which pin it is connected to.
When i connect the 2nd Motor, both motors gets stuck at one position. If i remove one of the motor, program works fine again.
This program controls the servo by serially communicating with a computer _ 'c' for center, 'r' for right and 'l' for left.
Compiler : Tasking
I have written a program to drive two servos simultaneously. Both Motors are programmed to have identical movement.
Motor 1 is connected to P7.0 & Motor 2 is connected to P7.2. The program works fine with only one motor regardless of which pin it is connected to.
When i connect the 2nd Motor, both motors gets stuck at one position. If i remove one of the motor, program works fine again.
This program controls the servo by serially communicating with a computer _ 'c' for center, 'r' for right and 'l' for left.
Code:
#include <regf276e.h>
_sfrbit LED4 _atbit (P3,9);
_sfrbit LED5 _atbit (P3,15);
int mspulse,pulse_value;
void Setup(void)
{
T78CON = 0x4042;
_putbit(1,DP3,9);
_putbit(1,DP3,15);
_putbit(1,DP7,0);
_putbit(1,DP7,2);
T7IC = 0xc4;
_bfld( DP3, 0x0c00, 0x0400 );
_putbit( 1, P3, 10 );
S0CON = 0x8011;
S0BG = 0x0067;
S0RIE = 1;
S0REN = 1;
IEN = 1;
}
_interrupt (0x3d) void Timer7_int (void)
{
LED5 =!LED5;
mspulse = pulse_value*2;
if (_getbit(P7,0) == 0)
{
_putbit(1,P7,0);
_putbit(1,P7,2);
T7=65536-mspulse; // 1 ms delay
}
else
{
_putbit(0,P7,0);
_putbit(0,P7,2);
T7= 65536 - (20000-mspulse); // 19 ms delay
}
}
void delay_us(unsigned int us)
{
unsigned int i;
if (us<1) return;
i = us*8;
T8 = 0;
while (T8<i)
{
_nop;
}
}
void delay_ms(unsigned int ms)
{
unsigned int i;
for (i=0;i<ms;i++)
delay_us(1000);
}
void Move_Left(void)
{
if (pulse_value <= 500) return;
pulse_value = pulse_value - 100;
}
void Move_Right(void)
{
if (pulse_value >= 2300) return;
pulse_value = pulse_value + 100;
}
void Serial_Tx(char* Tx)
{
while (*Tx)
{
while(!S0TBIR) {}
S0TBIR =0;
S0TBUF = *Tx++;
}
}
void main (void)
{
char R;
Setup();
pulse_value = 2300;
while(1)
{
while (!S0RIR)
{}
S0RIR = 0;
R = S0RBUF;
S0TBUF = R;
LED4 = !LED4;
switch (R)
{
case 'l':
Move_Left();
break;
case 'r':
Move_Right();
break;
case 'c':
pulse_value = 1400;
break;
}
}
}