void crayZED2() {
int sens;
sens = joystick_value();
while(sens == 1){
sens = joystick_value();
PORTD = 0x00;
}
for(int i = 0; i<4; i++){
_delay_ms(WAIT);
setCPin(STEP_4);
setDPin(STEP_4);
_delay_ms(WAIT);
setCPin(STEP_3);
setDPin(STEP_3);
_delay_ms(WAIT);
setCPin(STEP_2);
setDPin(STEP_2);
_delay_ms(WAIT);
setCPin(STEP_1);
setDPin(STEP_1);
}
}
void DynamixelClass::begin(long baud, unsigned char directionPin)
{
Direction_Pin = directionPin;
setDPin(Direction_Pin,OUTPUT);
beginCom(baud);
}
void reverseMotor( struct Motor* motor, int ticks) {
int i = 0;
for(i = 0; i< ticks; i++){
motor->pos--;
if (motor->pos == -1)
{
motor->pos = 3;
}
switch (motor->ID)
{
///MOTOR A///
case 0:
//PORTD &= ~(1<<0);
//PORTD &= ~(1<<1);
//PORTD &= ~(1<<2);
//PORTD &= ~(1<<3);
tickA--;
//printf_P(PSTR("tickA: %d\n"), tickA);
switch (motor->pos)
{
case 0:
setCPin(STEP_1);
break;
case 1:
setCPin(STEP_2);
break;
case 2:
setCPin(STEP_3);
break;
case 3:
setCPin(STEP_4);
break;
}
break;
///MOTOR B///
case 1:
PORTD &= ~(1<<4);
PORTD &= ~(1<<5);
PORTD &= ~(1<<6);
PORTD &= ~(1<<7);
tickB--;
//printf_P(PSTR("\ttickB: %d\n"), tickB);
switch (motor->pos)
{
case 0:
setDPin(STEP_1);
break;
case 1:
setDPin(STEP_2);
break;
case 2:
setDPin(STEP_3);
break;
case 3:
setDPin(STEP_4);
break;
}
break;
}
}
}
