void task_pid::run (void)
{
// Make a variable which will hold times to use for precise task scheduling
TickType_t previousTicks = xTaskGetTickCount ();
for (;;)
{
int16_t ref = setpoint->get();
//*p_serial << PMS("SetPoint: ") << ref << endl;
//*p_serial << PMS("p") << ref << endl;
if(data_read -> get() == 6)
{
*p_serial << ref << endl;
}
int16_t act = feedback->get();
// proportional
int16_t err = ref - act;
// integral
err_sum = sssub( ssadd(err, err_sum), ssdiv(ssmul(KW, windup), 1024) );
// derivative
int16_t err_deriv = act - old_act;
old_act = act;
int16_t err_tot =
ssadd(
ssdiv(ssmul(KP,err),1024),
ssadd(
ssdiv(ssmul(KI,err_sum),1024),
ssdiv(ssmul(KD,err_deriv),1024)
)
);
int16_t out;
// saturation limits
if (err_tot > MAX)
{
out = MAX;
}
else if (err_tot < MIN)
{
out = MIN;
}
else out = err_tot;
windup = sssub(err_tot, out);
motor_directive -> put(SETPOWER);
output->put(out);
if(data_read -> get() == 6)
{
*p_serial << output -> get() << endl;
}
// This is a method we use to cause a task to make one run through its task
// loop every N milliseconds and let other tasks run at other times
delay_from_for_ms (previousTicks, 5);
}
}
void task_brightness::run (void)
{
// Make a variable which will hold times to use for precise task scheduling
TickType_t previousTicks = xTaskGetTickCount ();
// Create an analog to digital converter driver object and a variable in which to
// store its output. The variable p_my_adc only exists within this run() method,
// so the A/D converter cannot be used from any other function or method
adc* p_my_adc = new adc (p_serial);
// Configure counter/timer 3 as a PWM for LED brightness. First set the data
// direction register so that the pin used for the PWM will be an output. The
// pin is Port E pin 4, which is also OC3B (Output Compare B for Timer 3)
DDRE = (1 << 4);
// To set 8-bit fast PWM mode we must set bits WGM30 and WGM32, which are in two
// different registers (ugh). We use COM3B1 and Com3B0 to set up the PWM so that
// the pin output will have inverted sense, that is, a 0 is on and a 1 is off;
// this is needed because the LED connects from Vcc to the pin.
TCCR3A |= (1 << WGM30) | (1 << COM3B1) | (1 << COM3B0);
// The CS31 and CS30 bits set the prescaler for this timer/counter to run the
// timer at F_CPU / 64
TCCR3B |= (1 << WGM32) | (1 << CS31) | (1 << CS30);
// This is the task loop for the brightness control task. This loop runs until the
// power is turned off or something equally dramatic occurs
for (;;)
{
// Read the A/D converter from channel 1
uint16_t a2d_reading = p_my_adc->read_once(1);
// Convert the A/D reading into a PWM duty cycle. The A/D reading is between 0
// and 1023; the duty cycle should be between 0 and 255. Thus, divide by 4
uint16_t duty_cycle = a2d_reading / 4;
//print the Duty_Cycle in comparison to the Power Signal being pumped into set_power
*p_serial <<PMS ("ADC Reading: ")<< a2d_reading <<dec <<endl
<<PMS ("Power_Signal: ")<< duty_cycle <<dec <<endl;
// Set the brightness. Since the PWM has already been set up, we only need to
// put a new value into the duty cycle control register, which on an AVR is
// the output compare register for a given timer/counter
OCR3B = duty_cycle;
//OCR1B = duty_cycle;
// Increment the run counter. This counter belongs to the parent class and can
// be printed out for debugging purposes
runs++;
// This is a method we use to cause a task to make one run through its task
// loop every N milliseconds and let other tasks run at other times
delay_from_for_ms (previousTicks, 1000);
}
}
void task_hctl_2000::run (void)
{
TickType_t prev = xTaskGetTickCount();
while (1)
{
uint16_t val = counter->read();
uint8_t low = counter->get_low();
uint8_t high = counter -> get_high();
*p_serial << PMS("Current encoder reading: ") << dec << val << " = " << bin << val << endl;
*p_serial << PMS("High Bits: ") << dec << high << " = " << bin << high << endl;
*p_serial << PMS("Low Bits: ") << dec << low << " = " << bin << low << endl;
delay_from_for_ms(prev, 1);
}
}
/**
* @brief Runs to update our shares with Heading, Pitch, Roll
*
* @details We want data that is reliable and that means the most
* IMMEDIATE data in my opinion, So We update every 100ms.
*/
void task_imu::run (void)
{
TickType_t previousTicks = xTaskGetTickCount ();
for (;;)
{
// Update each individual share, once per cycle
heading -> put(local_bno055_ptr -> getHeading());
roll -> put(local_bno055_ptr -> getRoll());
pitch -> put(local_bno055_ptr -> getPitch());
// Increment the run counter in the parent class.
runs++;
/* This is a method we use to cause a task to make one run through its
* task loop every 50 milliseconds and let other tasks run at other
* times
*/
delay_from_for_ms (previousTicks, 100);
}
}
void task_brightness::run (void)
{
// Make a variable which will hold times to use for precise task scheduling
TickType_t previousTicks = xTaskGetTickCount ();
// Create an analog to digital converter driver object and a variable in which to
// store its output. The variable p_my_adc only exists within this run() method,
// so the A/D converter cannot be used from any other function or method
adc* p_my_adc = new adc (p_serial);
//Constructs new motor for motor 1 on ATmega board
//motor_drv* p_motor_1 = new motor_drv(p_serial, &PORTC, 0, &PORTB, 6, &TCCR1A, &OCR1B);
//Constructs new motor for motor2 on ATmega board
motor_drv* p_motor_1 = new motor_drv(p_serial, &PORTD, 5, &PORTB, 5, &TCCR1A, &OCR1A);
// This is the task loop for the brightness control task. This loop runs until the
// power is turned off or something equally dramatic occurs
for (;;)
{
// Read the A/D converter
int16_t a2d_reading = p_my_adc->read_oversampled(0,10);//read_once (0);
// Convert the A/D reading into a PWM duty cycle. The A/D reading is between 0
// and 1023; the duty cycle should be between 0 and 255. Thus, divide by 4
int16_t torque;
if (a2d_reading < 425)
{
torque = (255 - (a2d_reading / 5) * 3);
torque = torque * -1;
}
else if (a2d_reading > 598)
{
a2d_reading = a2d_reading - 598 ;
torque = (a2d_reading / 5) * 3;
}
else
{
torque = 0;
}
// Set the brightness. Since the PWM has already been set up, we only need to
// put a new value into the duty cycle control register, which on an AVR is
// the output compare register for a given timer/counter
// Increment the run counter. This counter belongs to the parent class and can
// be printed out for debugging purposes
runs++;
// /*
// debug printing code for testing the output
if ((runs%10) == 0)
{
*p_serial << "PORTD : " << PORTD << endl;
*p_serial << "Duty Cycle : " << OCR1A << endl;
p_motor_1->set_power(torque); //Calls set_power on motor 1
*p_serial << "Torque : " << torque << endl;
*p_serial << "Duty Cycle : " << OCR1A << endl;
*p_serial << endl;
}
// */
// This is a method we use to cause a task to make one run through its task
// loop every N milliseconds and let other tasks run at other times
delay_from_for_ms (previousTicks, 100);
}
}
void task_encoder::run (void)
{
// Make a variable which will hold times to use for precise task scheduling
TickType_t previousTicks = xTaskGetTickCount ();
// Configure counter/timer 1 as a PWM for Motor Drivers.
// COM1A1/COM1B1 to set to non inverting mode.
// WGM10 to set to Fast PWM mode (only half ughhhhh)
TCCR1A |= (1 << WGM10) | (1 << COM1A1) | (1 << COM1B1);
// This is the second Timer/Counter Register
// WGM12 (other half of Fast PWM)
// CS11 Sets the presacler to 8 (010)
TCCR1B |= (1 << WGM12) | (1 << CS11);
// Create an analog to digital converter driver object and a variable in which to
// store its output. The variable p_my_adc only exists within this run() method,
// so the A/D converter cannot be used from any other function or method
adc* p_adc = new adc (p_serial);
//** CREATE THE MOTOR DRIVERS **//
// create a pointer to a motor driver object and pass it addresses to PORTC, PORTB, OC1B and Pin Values for PC0, PC2, and PB6 as the PWM
motor_driver* p_motor1 = new motor_driver(p_serial, &PORTC, &PORTC, &PORTB, &OCR1B, PC0, PC1, PC2, PB6);
// create a pointer to a motor driver object and pass it addresses to PORTD, PORTB, OC1A and Pin Values for PD5, PD7, and PB5 as the PWM
motor_driver* p_motor2 = new motor_driver(p_serial, &PORTD, &PORTD, &PORTB, &OCR1A, PD5, PD6, PD7, PB5);
// The loop to contunially run the motors
while (1)
{
// Read the A/D converter from channel 1
uint16_t a2d_reading = p_adc->read_once(1);
// Convert the A/D reading into a PWM duty cycle. The A/D reading is between 0
// and 1023; the duty cycle should be between 0 and 255. Thus, divide by 4
uint16_t duty_cycle = a2d_reading / 4;
int16_t power = ((int16_t)duty_cycle * 1);
//doesn't let the duty_cycle go to 0 because then the power is 0.
if (duty_cycle == 0) {duty_cycle++;}
//From 00 - 64 we want to decrease forwards
//From 65 - 128 we want to increase backwards
//From 129 - 192 we want to set both outs to high to set holding brake
//From 193 - 255 we want to set both outs to low so that they don't affect the motor
if (duty_cycle <= 64)
{
//forwards start from high speed to go to low speed
power = 255 - (4 * duty_cycle);
p_motor1->set_power(power);
p_motor2->set_power(duty_cycle);
}
else if (duty_cycle > 64 && duty_cycle <= 128)
{
//backwards start from low speed to go to high speed
power = -4 * (duty_cycle - 64);
p_motor1->set_power(power);
p_motor2->set_power(-1 * duty_cycle);
}
else if ( duty_cycle > 128 && duty_cycle <= 192)
{
//power stopping
power = 4 * (duty_cycle - 128);
p_motor1->brake(power);
p_motor2->brake(power);
}
else
{
//free wheeling
p_motor1->brake();
p_motor2->brake();
}
*p_serial << PMS ("Duty_Cycle: ") << duty_cycle << dec << endl
<< PMS ("Power_Signal: ") << power << dec << endl;
// Increment the run counter. This counter belongs to the parent class and can
// be printed out for debugging purposes
runs++;
// This is a method we use to cause a task to make one run through its task
// loop every N milliseconds and let other tasks run at other times
delay_from_for_ms (previousTicks, 10);
}
}
void task_brightness::run (void)
{
// Make a variable which will hold times to use for precise task scheduling
TickType_t previousTicks = xTaskGetTickCount ();
// Create an analog to digital converter driver object and a variable in which to
// store its output. The variable p_my_adc only exists within this run() method,
// so the A/D converter cannot be used from any other function or method
adc* p_my_adc = new adc (p_serial);
//** CREATE THE MOTOR DRIVERS **//
// create a pointer to a motor driver object and pass it addresses to PORTC, PORTB, OC1B and Pin Values for PC0, PC2, and PB6 as the PWM
motor_driver* p_motor1 = new motor_driver(p_serial, &PORTC, &PORTC, &PORTB, &OCR1B, PC0, PC1, PC2, PB6);
// create a pointer to a motor driver object and pass it addresses to PORTD, PORTB, OC1A and Pin Values for PD5, PD7, and PB5 as the PWM
motor_driver* p_motor2 = new motor_driver(p_serial, &PORTD, &PORTD, &PORTB, &OCR1A, PD5, PD6, PD7, PB5);
// Configure counter/timer 3 as a PWM for LED brightness. First set the data
// direction register so that the pin used for the PWM will be an output. The
// pin is Port E pin 4, which is also OC3B (Output Compare B for Timer 3)
DDRE = (1 << 4);
// To set 8-bit fast PWM mode we must set bits WGM30 and WGM32, which are in two
// different registers (ugh). We use COM3B1 and Com3B0 to set up the PWM so that
// the pin output will have inverted sense, that is, a 0 is on and a 1 is off;
// this is needed because the LED connects from Vcc to the pin.
TCCR3A |= (1 << WGM30) | (1 << COM3B1) | (1 << COM3B0);
// The CS31 and CS30 bits set the prescaler for this timer/counter to run the
// timer at F_CPU / 64
TCCR3B |= (1 << WGM32) | (1 << CS31) | (1 << CS30);
// Configure counter/timer 1 as a PWM for Motor Drivers.
// COM1A1/COM1B1 to set to non inverting mode.
// WGM10 to set to Fast PWM mode (only half ughhhhh)
TCCR1A |= (1 << WGM10) | (1 << COM1A1) | (1 << COM1B1);
// This is the second Timer/Counter Register
// WGM12 (other half of Fast PWM)
// CS11 Sets the presacler to 8 (010)
TCCR1B |= (1 << WGM12) | (1 << CS11);
// This is the task loop for the brightness control task. This loop runs until the
// power is turned off or something equally dramatic occurs
for (;;)
{
// Read the A/D converter from channel 1
uint16_t a2d_reading = p_my_adc->read_once(1);
// Convert the A/D reading into a PWM duty cycle. The A/D reading is between 0
// and 1023; the duty cycle should be between 0 and 255. Thus, divide by 4
uint16_t duty_cycle = a2d_reading / 4;
int16_t power = ((int16_t)duty_cycle * 1);
//doesn't let the duty_cycle go to 0 because then the power is 0.
if(duty_cycle == 0){duty_cycle++;}
//From 00 - 64 we want to decrease forwards
//From 65 - 128 we want to increase backwards
//From 129 - 192 we want to set both outs to high to set holding brake
//From 193 - 255 we want to set both outs to low so that they don't affect the motor
if(duty_cycle <= 64)
{
//forwards start from high speed to go to low speed
power = 255 - (4 * duty_cycle);
p_motor1->set_power(power);
p_motor2->set_power(duty_cycle);
}
else if(duty_cycle > 64 && duty_cycle <= 128)
{
//backwards start from low speed to go to high speed
power = -4 * (duty_cycle - 64);
p_motor1->set_power(power);
p_motor2->set_power(-1*duty_cycle);
}
else if( duty_cycle > 128 && duty_cycle <= 192)
{
//power stopping
power = 4 * (duty_cycle - 128);
p_motor1->brake(power);
p_motor2->brake(power);
}
else
{
//free wheeling
p_motor1->brake();
p_motor2->brake();
}
//print the Duty_Cycle in comparison to the Power Signal being pumped into set_power
*p_serial <<PMS ("Duty_Cycle: ")<< duty_cycle <<dec <<endl
<<PMS ("Power_Signal: ")<< power <<dec <<endl;
// Set the brightness. Since the PWM has already been set up, we only need to
// put a new value into the duty cycle control register, which on an AVR is
// the output compare register for a given timer/counter
OCR3B = duty_cycle;
//OCR1B = duty_cycle;
// Increment the run counter. This counter belongs to the parent class and can
// be printed out for debugging purposes
runs++;
// This is a method we use to cause a task to make one run through its task
// loop every N milliseconds and let other tasks run at other times
delay_from_for_ms (previousTicks, 10);
}
}
