void Mech_Init(void)
{
RC_Init();
RC_AddPins(GRAB_PIN);
RC_SetPulseTime(GRAB_PIN, GRAB_UP_TIME);
}
void set_ESC_pulse(unsigned int pulse_width) {
if (pulse_width > 2000)
pulse_width = 2000;
if (pulse_width < 1000)
pulse_width = 1000;
RC_SetPulseTime(ESC_PIN, pulse_width);
}
void init_ESC_pulse(void) {
// currently using RCpin x4 as defined by RC_Servo.h
//look to IO_ports.h to figure out what pin this corresponds to on the micro
//corresponds to B0 on the micro and J5 15 on the UNO32
RC_Init();
RC_AddPins(ESC_PIN);
int i = 0;
for (i; i < 1000000; i++);
RC_SetPulseTime(ESC_PIN, 1500); //set intial speed to 0 rpm
}
void Mech_Grabber(uint8_t dir)
{
if (dir == GRAB_INIT)
{
RC_SetPulseTime(GRAB_PIN, GRAB_INIT_TIME);
}
else if (dir == GRAB_DOWN)
{
RC_SetPulseTime(GRAB_PIN, GRAB_DOWN_TIME);
}
else if (dir == GRAB_UP)
{
RC_SetPulseTime(GRAB_PIN, GRAB_UP_TIME);
}
else if (dir == SHAKE_UP)
{
RC_SetPulseTime(GRAB_PIN, SHAKE_UP_TIME);
}
else if (dir == SHAKE_DOWN)
{
RC_SetPulseTime(GRAB_PIN, SHAKE_DOWN_TIME);
}
}
/**
* Function: Gate_Open
* @return SUCCESS or ERROR
* @remark Sets the Gate wheel to the closed position
*/
char Gate_Open() {
RC_SetPulseTime(SERVO, OPEN_WIDTH);
//dbprintf("\nGate released (%d)", OPEN_WIDTH);
return SUCCESS;
}
/**
* Function: Gate_Close
* @return SUCCESS or ERROR
* @remark Sets the Gate wheel to the loading position
*/
char Gate_Close() {
RC_SetPulseTime(SERVO, CLOSE_WIDTH);
//dbprintf("\nGate closed (%d)", CLOSE_WIDTH);
return SUCCESS;
}
void Mech_Test(void)
{
char c = 0;
int pulsetime = GRAB_INIT_TIME;
char modified = 0;
printf("Servo test\n");
printf("Instructions: 'a'->+1; 'b'->+10; 'c'->100; the capital cases do the minus operation;\n");
printf("'u'->UP position; 'd'->DOWN position\n");
printf("Instructions: pulsetime range %d ~ %d\n", MINPULSE, MAXPULSE);
printf("pulsetime = %d\n", pulsetime);
while(c != 'q')
{
c = getchar();
switch(c)
{
case 'a':
pulsetime++;
modified = 1;
break;
case 'b':
pulsetime += 10;
modified = 1;
break;
case 'c':
pulsetime += 100;
modified = 1;
break;
case 'A':
pulsetime--;
modified = 1;
break;
case 'B':
pulsetime -= 10;
modified = 1;
break;
case 'C':
pulsetime -= 100;
modified = 1;
break;
case 'i':
pulsetime = GRAB_INIT_TIME;
modified = 1;
break;
case 'd':
pulsetime = GRAB_DOWN_TIME;
modified = 1;
break;
case 'u':
pulsetime = GRAB_UP_TIME;
modified = 1;
break;
}
if (modified)
{
if (pulsetime >= MINPULSE)
{
RC_SetPulseTime(GRAB_PIN, pulsetime);
}
printf("pulsetime = %d\n", pulsetime);
modified = 0;
}
}
printf("Mechanical parts pest ended\n");
}
int main(void) {
// ----------------- Initialization --------------
SERIAL_Init();
AD_Init(POT_INPUT);
// Initialize interrupts
INTEnableSystemMultiVectoredInt();
RC_Init(RC_PORT);
unsigned int wait = 0;
for (wait = 0; wait <= 1000000; wait++)
asm("nop");
RC_SetPulseTime(RC_PORT, 2000);
printf("\nHello,...");
while (1) {
// Read and print potentiometer
unsigned int potValue = ReadPotentiometer();
//printf("\nPot. reading: %x", potValue);
// Pause if desired
#ifdef ADC_PAUSE
#endif
unsigned int newSpeed = potValue +1000;
printf("\nPot value to %u", potValue);
// bound it
newSpeed = max(newSpeed,MINPULSE);
newSpeed = min(newSpeed,MAXPULSE);
// effect the motor
if (RC_SetPulseTime(RC_PORT, newSpeed) == SUCCESS) {
printf("\nSuccessfully set PWM to %u", newSpeed);
}
else {
printf("\nFailed to set PWM to %u", newSpeed);
}
/*
char keyPressed = GetChar();
if (keyPressed != 0) {
/*
if (keyPressed == 'f') {
// forward
printf("\nforward");
if (DIRECTION != FORWARD)
DIRECTION = FORWARD;
}
else if(keyPressed == 'r') {
// reverse
printf("\nreverse");
if (DIRECTION != REVERSE)
DIRECTION = REVERSE;
}
else if(keyPressed == 'q') {
printf("\nGoodbye!");
PWM_End();
return 0;
}
if(keyPressed == 't') {
unsigned short int stringMax = 4;
unsigned short int i = 0;
char charNumber[1];
charNumber[0] = 0;
char stringNumber[stringMax+1];
printf("\nPlease enter a frequency:");
while(charNumber[0] != 46) {
charNumber[0] = GetChar();
//printf("\nGot %s", charNumber);
if ((charNumber[0] <= 57 && charNumber[0] >= 48)) {
stringNumber[i] = charNumber[0];
stringNumber[i+1] = '\0';
//printf("\nAppended %c to '%s'",charNumber[0],stringNumber);
//stringNumber = strcat(stringNumber, charNumber);
i++;
}
if (i >= stringMax)
break;
}
//printf("\ni=%u stmax=%u", i, stringMax);
printf("\nDone");
unsigned int newFreq = atoi(stringNumber);
newFreq = max(newFreq,MINPULSE);
newFreq = min(newFreq,MAXPULSE);
printf("\nPulse width set to %u",newFreq);
RC_SetPulseTime(RC_PORT,newFreq);
}
} // end of keyPressed
*/
while (!IsTransmitEmpty()); // bad, this is blocking code
} // end of while loop
return 0;
}