int main() // main function
{
// Proportional control
servo_angle(16, 650); // 7 ticks/sec / 20 ms
while(1) // main loop
{
//pause(200);
distance = ping_cm(17);
//print("distance= %d\n", distance);
pause(200);
if (distance >25)
{
drive_speed(75,75);
}
else
{
drive_speed(0,0);
servo_angle(16, 10);
pause(1000);
int distanceleft = ping_cm(17);
pause(1000);
//print("distance left= %d\n", distanceleft);
servo_angle(16, 1600);
pause(1000);
//print("distance= %d\n", distanceleft);
int distanceright = ping_cm(17);
pause(1000);
//print("distance right= %d\n", distanceright);
servo_angle(16, 650);
pause(1000);
//print("distance= %d\n", distanceleft);
//servo_angle(16, 750);
int a = distanceleft - distanceright;
//pause(200);
//print("a= %d\n", a);
pause(200);
if (a == 0)
{
drive_speed(64,-64);
pause(300);
print("1\n");
}
else if (a > 0)
{
drive_speed(-64,64);
pause(150);
print("2\n");
}
else if(a < 0)
{
drive_speed(64,-64);
pause(150);
print("3\n");
}
}
// Calculate correction speed
}
}
int main() // main function
{
servo_angle(16, 0); // P16 servo to 0 degrees
pause(500); // Allow 1/2 second to get there
servo_setramp(16, 7); // Change by up to 7/10 degree/20 ms
servo_angle(16, 1800); // P16 servo to 180 degrees
pause(6000); // Allow 6 seconds to get there
// (with ramping in effect)
servo_stop(); // Stop servo process
}
// init servo signal and pin
// does not check arguments, make sure to pass valide arguments
void servo_init(void)
{
xSysCtlPeripheralClockSourceSet(xSYSCTL_PWMB_MAIN, 1);
// Enable PWM peripheral
xSysCtlPeripheralEnable(SYSCTL_PERIPH_PWMB);
// Enable gpio pin peripheral
xSysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
// Enable PWM and set GPIO Pin as PWM
xSPinTypePWM(TIM1CH0, PB0);
// xSPinTypePWM(TIM0CH3, PE30);
// Set invert, dead zone and mode
xPWMInitConfigure(xPWMB_BASE, xPWM_CHANNEL0, xPWM_TOGGLE_MODE);
// Set CNR, Prescale and Divider. FHz = 50Hz
xPWMFrequencySet(xPWMB_BASE, xPWM_CHANNEL0, 50);
// Set CMR, high pulse @ 1,5 ms middle position
servo_angle(0);
// Set output enable
xPWMOutputEnable(xPWMB_BASE, xPWM_CHANNEL0);
// start pwm
xPWMStart(xPWMB_BASE, xPWM_CHANNEL0);
// initialization ok
}
void Grip(int gripTicks, int gripSpeed)
{
servo_setramp(16, 32);
servo_angle(16, gripTicks);
//dprint(term, "Using Gripper");
//pause(gripSpeed);
gripState = -1;
}
