void pwm_run()
{
servo_start();
waitcnt(CNT + CLKFREQ/2);
while(1)
{
for (int i=0;i<4;i++)
servo_set(motors[i]->pin, motors[i]->current_val);
}
}
int servo_set(int pin, int time) // Set pulse width to servo on pin
{
if(servoCog == 0) // If cog not started
{
int result = servo_start(); // Start the cog
if(result == 0) return 0; // No cogs open
if(result == -1) return -1; // No locks open
}
int s = sizeof(p)/sizeof(int); // Array size to s
int i; // Index variable
while(lockset(lockID)); // Set the lock
for(i = 0; i < s; i++) // Check if existing servo
{
if(p[i] == pin) // Yes?
{
t[i] = time; // Set pulse duration
lockclr(lockID); // Clear lock
return 1; // Return success
}
}
if(i == s) // Not existing servo?
{
for(i= 0; i < s; i++) // Look for empty slot
{
if(p[i]==-1) // Found one?
{
break; // Exit for loop, keep index
}
}
if(i <= s) // Found empty slot?
{
p[i] = pin; // Set up pin and pulse durations
t[i] = time;
tp[i] = time;
lockclr(lockID); // Clear the lock
return 1; // Return success
}
else // Servo not found, no empty slots?
{
lockclr(lockID); // Clear lock
return 0; // Return, set not completed
}
}
}
int window_fan() {
rcc_config();
delay_config();
led_debug_config();
motor_config();
led_blue_off();
led_green_off();
servo_config();
servo_set_pos(0);
servo_start();
u32 i;
u32 from = 0;
u32 to = 180;
u32 delay = 2000;
while(1) {
motor_forward();
led_blue_on();
led_green_off();
for(i=from; i<to; i++) {
servo_set_pos(i);
delay_ms(delay);
}
led_blue_off();
led_green_on();
for(i=to; i>from; i--) {
servo_set_pos(i);
delay_ms(delay);
}
motor_stop();
delay_ms(10000);
}
}