void LEGOPowerFunctions::ComboPWM(int blue_pwm, int red_pwm, int channel) {
int nib1, nib2, nib3, nib4;
//set nibs
nib1 = 0x4 | channel;
nib2 = blue_pwm;
nib3 = red_pwm;
nib4 = 0xf ^ nib1 ^ nib2 ^ nib3;
message_pause(channel, messagecount);
pf_send(nib1 << 4 | nib2, nib3 << 4 | nib4);
}
void LEGOPowerFunctions::ComboMode(int blue_speed, int red_speed, int channel) {
int nib1, nib2, nib3, nib4;
//set nibs
nib1 = channel;
nib2 = 0x01;
nib3 = blue_speed | red_speed;
nib4 = 0xf ^ nib1 ^ nib2 ^ nib3;
message_pause(channel, messagecount);
pf_send(nib1 << 4 | nib2, nib3 << 4 | nib4);
}
/*-------------------------------------------------------------------------*/
void *scheduler_get(scheduler_t *sched, int id) {
void *task = 0;
pthread_mutex_t *mutex = sched->mutex;
pthread_cond_t *cond = sched->cond;
list_t **queue = sched->queue;
/* We wait until an executable task is available. Although the
semaphore might suggest that there are still some tasks in the
queues, there might be actually nothing left. This might happen
if, for example, during a pause, the scheduler has been
reset. We therefore add a while in order to guaranty that when
the function exits, a task is returned. */
while (task == 0) {
sem_wait(sched->sem);
pthread_mutex_lock(mutex);
/* If we are in pause mode, then we wait until the conditional
variable is signaled. */
if (sched->pause) {
message_pause();
pthread_cond_wait(cond, mutex);
}
/* Pick a task from the high priority queue if it is not empty. */
if (queue[high]->size > 0)
task = list_rem_head(queue[high]);
/* Otherwise, if the disk queue is not empty and nobody is
accessing the disk at the moment, then pick a task from the
disk queue. */
else if (queue[disk]->size > 0 && sched->disk == -1) {
sched->disk = id;
task = list_rem_head(queue[disk]);
}
/* Finally, if none of the above cases where taken, then pick a
task from the low priority queue. */
else
task = list_rem_head(queue[low]);
pthread_mutex_unlock(mutex);
}
return task;
}
void LEGOPowerFunctions::SinglePin(int mode, int function, int pin, int output, int channel) {
int nib1, nib2, nib3, nib4;
//set nibs
nib1 = toggle[channel] | channel;
nib2 = 0x00 | mode;
nib3 = output << 4 | pin << 3 | function;
nib4 = 0xf ^ nib1 ^ nib2 ^ nib3;
message_pause(channel, messagecount);
pf_send(nib1 << 4 | nib2, nib3 << 4 | nib4);
if (toggle[channel] == 0)
toggle[channel] = 8;
else
toggle[channel] = 0;
}
void LEGOPowerFunctions::SingleOutput(int mode, int step, int output, int channel) {
int nib1, nib2, nib3, nib4;
//set nibs
nib1 = toggle[channel] | channel;
nib2 = 0x4 | mode | output;
nib3 = step;
nib4 = 0xf ^ nib1 ^ nib2 ^ nib3;
message_pause(channel, messagecount);
pf_send(nib1 << 4 | nib2, nib3 << 4 | nib4);
if (toggle[channel] == 0)
toggle[channel] = 8;
else
toggle[channel] = 0;
}
