Last active
May 25, 2018 06:06
-
-
Save nmannan97/8ada391c624bf92057dfef0aa1bfcad8 to your computer and use it in GitHub Desktop.
RTOS lab to make operating system for a PID controlled motor. One button would make a motor run at 1500 RPM with PID. A second button would control the position. The last would allow for free movement of the motor.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//***************Lab 7*************/ | |
/*Notes: | |
~Mutex = mutual exclusion | |
~A semaphore that is used for mutual exclusion must always be returned | |
~A mutex is a type of semaphore | |
~To create a mutex type semaphore, use the xSemaphoreCreateMutex() API function. | |
~xSemaphoreTake( xMutex, portMAX_DELAY ); | |
~ The following line will only execute once the mutex has been successfully obtained. | |
~xSemaphoreGive( xMutex ); The mutex MUST be given back! | |
~main() simply creates the mutex, creates the tasks, then starts the scheduler | |
~ | |
*/ | |
// | |
// | |
// | |
// | |
// Completed with lab partners Zach Stow and Chang Lee | |
// | |
// | |
// | |
// | |
// | |
#include <FreeRTOS.h> | |
#include <asf.h> | |
#include <task.h> | |
//Global Functions | |
void init_TC(void); | |
void init_Interrupts(void); | |
void init_EIC(void); | |
void init_adc(void); | |
int read_adc(void); | |
void display(void); | |
void SSD_display(char); | |
void array_conversion(int); | |
void Simple_Clock_Init(void); | |
char input_detection(void); | |
void enable_clocks(void); | |
void init_ports(void); | |
void Medium_Priority(void); | |
void Lowest_Priority(void); | |
float PID(float); | |
float PID2(float); | |
void wait(int); | |
//Global Pointers | |
TcCount8* TC_0 = &(TC0->COUNT8); | |
TcCount8* TC_4 = &(TC4->COUNT8); | |
TcCount8* TC_6 = &(TC6->COUNT8); | |
PortGroup *porA = (PortGroup *)PORT; | |
PortGroup *porB = (PortGroup *)PORT+1; | |
//Global Variables | |
volatile int count = 0; | |
volatile char num[4]; | |
volatile float y, y_1, u, u_1; | |
volatile int state = 1; | |
volatile int output = 0; | |
volatile int position = 0, rpm, lag = 0, counter = 0; | |
volatile int desired_rpm = 0; | |
volatile int speed_control, derrivative, new_speed, current_speed, speed_past, past_position, position_correct, current_pos, pos_error; | |
int PI_speed = 0; | |
int p = 0; | |
float Ts = 0.005; //0.0015 | |
float Kp = 0.0005; //0.00122; - 5.2 sec to 1500 rpm, 4.10 to 0 rpm | |
float Ki = 0.00011;//0.0005; | |
float kd = 0; //Need for position control | |
float kff = 0; | |
float error = 0.0; | |
float integral = 0; | |
int col_input[4] = {PORT_PA16,PORT_PA17,PORT_PA18,PORT_PA19}; | |
int row_input[4] = {PORT_PA04,PORT_PA05,PORT_PA06,PORT_PA07}; | |
SemaphoreHandle_t xtask1 = NULL; | |
SemaphoreHandle_t xtask2 = NULL; | |
int main (void) | |
{ | |
//taskENABLE_INTERRUPTS(); | |
porB->DIRSET.reg = PORT_PB09; | |
porB->OUTSET.reg = PORT_PB09; | |
Simple_Clock_Init(); | |
enable_clocks(); | |
init_ports(); | |
init_adc(); | |
init_EIC(); | |
init_TC(); | |
init_Interrupts(); | |
//Set PA04 to PA07 as | |
for (int i = 4; i < 8; i++) | |
porA->DIRSET.reg = (1u << i); | |
//Set PB00 to PB07 as | |
for (int i = 0; i < 8; i++) | |
porB->DIRSET.reg = (1u << i); | |
//Set PA16 to PA19 as | |
for (int i = 16; i < 20; i++) | |
{ | |
porA->DIRCLR.reg = (1u << i); | |
porA->PINCFG[i].reg = PORT_PINCFG_INEN | PORT_PINCFG_PULLEN; | |
} | |
// Before a semaphore is used it must be explicitly created | |
xtask1 = xSemaphoreCreateMutex(); //create Semaphore | |
xtask2 = xSemaphoreCreateMutex(); //create Semaphore | |
// Check the semaphore was created successfully. configASSERT() is described in section 11.2. | |
configASSERT(xtask1); | |
//configASSERT(xtask2); | |
xTaskCreate(Medium_Priority, "Keypad/Display", 1024, NULL, 1, NULL); | |
xTaskCreate(Lowest_Priority, "Motor/Pid Control", 1024, NULL, 2, NULL); | |
//Start the scheduler so the created tasks start executing. | |
vTaskStartScheduler(); | |
} | |
/* | |
~Medium Priority | |
~200Hz | |
~Low-pass filter | |
~speed and/or position control | |
*/ | |
void Medium_Priority() | |
{ | |
const TickType_t hz_1 = (1000/200); | |
// As per most tasks, this task is implemented as an infinite loop. | |
while(1) | |
{ | |
// Obtain the mutex that is protecting access to the display. | |
if(xSemaphoreTake(xtask1, portMAX_DELAY)) | |
{ | |
volatile float yee; | |
//volatile float correct; | |
u = position*15; //position is position of motor | |
//if (u < 0) | |
//u = -u; | |
// Run through FIR filter | |
y = 0.03093*u_1 + 0.9691*y_1; | |
// Recursive assignment | |
y_1 = y; | |
u_1 = u; | |
rpm = y; | |
//speed_control = PID(u); | |
yee = position; | |
past_position = yee; | |
//position = 0; //if commented out displays current position, but PID becomes erratic | |
if(state == 2 || state == 4){ | |
speed_control = PID(u); | |
while(TC_4->STATUS.bit.SYNCBUSY); | |
position = 0; | |
TC_4->CC[0].reg = speed_control; | |
TC_4->CC[1].reg = 0; | |
} | |
else if(state == 1){ | |
speed_control = PID(u); | |
while(TC_4->STATUS.bit.SYNCBUSY); | |
position = 0; | |
TC_4->CC[0].reg = 0; //hard code 0 RPM in state 1 (idle) | |
TC_4->CC[1].reg = 0; | |
} | |
else if(state == 3){ | |
speed_control = PID(u); | |
while(TC_4->STATUS.bit.SYNCBUSY); | |
position = 0; | |
TC_4->CC[0].reg = 34; //hard code 1500RPM in state 3 (speed control) | |
TC_4->CC[1].reg = 0; | |
} | |
else if(state == 5){ | |
position_correct = PID2(past_position); | |
if (past_position < -10){ | |
porB->OUTCLR.reg = PORT_PB09; | |
TC_4->CC[0].reg = abs(position_correct); | |
TC_4->CC[1].reg = 0; | |
} | |
if(past_position > 10){ | |
porB->OUTSET.reg = PORT_PB09; | |
TC_4->CC[0].reg = 0; | |
TC_4->CC[1].reg = abs(position_correct); | |
} | |
if(past_position>-10 && past_position <10) { | |
//position = 0; | |
TC_4->CC[0].reg = 255; | |
TC_4->CC[1].reg = 255; | |
} | |
} | |
//vTaskSuspend(Medium_Priority); | |
xSemaphoreGive(xtask1); | |
} | |
vTaskDelay(hz_1); | |
} | |
} | |
float PID(float uu){ | |
error = desired_rpm - uu; | |
integral += error; | |
derrivative = (error - speed_past)*200; | |
new_speed = Kp*error + Ki*integral + kd*derrivative; //PID sum; | |
current_speed = new_speed; //setting a new speed | |
speed_past = current_speed; // setting up a temp speed | |
return current_speed; | |
} | |
float PID2(float past_position){ //Position Control | |
float Kpp = 0.0002; //0.0005; | |
float Kii = 0.00025; //0.00011; | |
float kdd = 6.25e-05; //0.05; //Need for position control | |
error = 0 - past_position; | |
integral += error; | |
derrivative = (error - speed_past)*200; | |
new_speed = Kpp*error + Kii*integral + kdd*derrivative; //PID sum; | |
current_speed = new_speed; //setting a new speed | |
speed_past = current_speed; // setting up a temp speed | |
return current_speed; | |
} | |
/* | |
~lowest Priority | |
~60Hz | |
~7-segment display | |
~Keypad interface | |
~state machine | |
*/ | |
void Lowest_Priority() //60Hz | |
{ | |
const TickType_t hz_2 = (1000/60); | |
while(1) | |
{ | |
if(xSemaphoreTake(xtask2, portMAX_DELAY)) | |
{ | |
if(!(state == 5)){ | |
array_conversion(abs(rpm)); | |
display(); | |
} | |
if(state == 5){ | |
array_conversion(abs(past_position)); | |
display(); | |
} | |
unsigned char check = input_detection(); | |
//State Machine: 1 = idle, 2 = accelerate, 3 = speed control, 4 = Decelerate, 5 = position control | |
if(check == '0' || check =='1' || check =='2'){ | |
if(state == 1 && check == '1'){ | |
state = 2; | |
} | |
else if(state == 2 && check == '2'){ | |
state = 1; | |
} | |
else if(state == 5 && check == '1'){ | |
state = 2; | |
kd = 0; | |
} | |
else if(state == 5 && check == '0'){ | |
state = 4; | |
kd = 0; | |
integral = 0; | |
derrivative = 0; | |
new_speed = 0; | |
current_speed = 0; | |
speed_past = 0; | |
new_speed = 0; | |
} | |
else if(state == 1 && check == '2'){ | |
state = 5; | |
error = 0; | |
integral = 0; | |
derrivative = 0; | |
new_speed = 0; | |
current_speed = 0; | |
speed_past = 0; | |
new_speed = 0; | |
} | |
switch(check){ | |
case '0': {desired_rpm = 0; | |
break;} | |
case '1': {desired_rpm = 1500; | |
break;} | |
case '2': {desired_rpm = 0; | |
if(rpm == 0){ | |
state = 5; | |
} | |
break;} | |
default : {desired_rpm = 0;} | |
} | |
} | |
xSemaphoreGive(xtask2); | |
} | |
vTaskDelay(hz_2); //Sets interrupt frequency | |
} | |
} | |
void EIC_Handler(void){ | |
// Booleans | |
volatile bool phaseA = (porA->IN.reg & (0x1 << 28)); | |
volatile bool phaseB = (porB->IN.reg & (0x1 << 14)); | |
if(phaseA && phaseB) | |
position--; | |
else if(phaseA || phaseB) | |
position++; | |
else | |
position--; | |
// Clear Interrupt Flag | |
EIC->INTFLAG.reg = 0xFFFF; | |
} | |
char input_detection(void) | |
{ | |
volatile char x; | |
porA -> DIRSET.reg = PORT_PA07 | PORT_PA06| PORT_PA05| PORT_PA04; | |
porA -> DIRCLR.reg = PORT_PA19 | PORT_PA18 | PORT_PA17 | PORT_PA16; | |
//Initialization of keypad matrix | |
unsigned char keypad_key[4][4] = {{'D','C','B','A'}, | |
{'#','9','6','3'}, | |
{'0','8','5','2'}, | |
{'*','7','4','1'}}; | |
/* Scan the keypad to find which key has been pressed */ | |
for (int row = 0; row <= 3; row++) | |
{ | |
porA -> OUTSET.reg = PORT_PA04|PORT_PA05|PORT_PA06|PORT_PA07; | |
porA -> OUTCLR.reg = row_input[row]; | |
for (int col = 0; col <= 3; col ++) | |
{ | |
if((porA->IN.reg) & col_input[col]) | |
counter++; | |
{ | |
if (counter > 10) | |
{ | |
x = keypad_key[col][row]; | |
counter = 0; //Reset counter | |
return x; //Return char that was detected | |
} | |
} | |
} | |
} | |
} | |
void enable_clocks(void) | |
{ | |
// Enable APB Clock | |
PM->APBCMASK.bit.ADC_ = 0x1; | |
PM->APBCMASK.bit.TC0_ = 0x1; | |
PM->APBCMASK.bit.TC4_ = 0x1; | |
PM->APBCMASK.bit.TC6_ = 0x1; | |
// ADC | |
GCLK->CLKCTRL.bit.ID = 0x17; | |
GCLK->CLKCTRL.reg |= (0x1 << 14); | |
// TC0 | |
GCLK->CLKCTRL.bit.ID = 0x13; | |
GCLK->CLKCTRL.reg |= (0x1 << 14); | |
// TC4 | |
GCLK->CLKCTRL.bit.ID = 0x15; | |
GCLK->CLKCTRL.reg |= (0x1 << 14); | |
// TC6 | |
GCLK->CLKCTRL.bit.ID = 0x16; | |
GCLK->CLKCTRL.reg |= (0x1 << 14); | |
// EIC | |
GCLK->CLKCTRL.bit.ID = 0x3; | |
GCLK->CLKCTRL.reg |= (0x1 << 14); | |
} | |
void init_ports(void) | |
{ | |
// Set PA_11 to ADC Peripheral | |
porA->PINCFG[11].bit.PMUXEN = 0x1; | |
porA->PMUX[5].bit.PMUXO = 0x1; | |
// SET PA_28 to EIC Peripheral | |
porA->PINCFG[28].bit.PMUXEN = 0x1; | |
porB->PINCFG[14].bit.PMUXEN = 0x1; | |
porA->PMUX[14].bit.PMUXE = 0x0; | |
// Port PA_22 to TC | |
porA->PINCFG[22].bit.PMUXEN = 0x1; | |
porA->PMUX[11].bit.PMUXE = 0x5; // TC Peripheral | |
// Port PA_23 to TC | |
porA->PINCFG[23].bit.PMUXEN = 0x1; | |
porA->PMUX[11].bit.PMUXO = 0x5; // TC Peripheral | |
// Display | |
porA->DIRSET.reg = 0xF0; | |
porB->DIRSET.reg = 0xFF; | |
// EIC | |
porB->DIRCLR.reg = (0x1 << 14); | |
porA->DIRCLR.reg = (0x1 << 28); | |
} | |
void init_Interrupts(void){ | |
// Enable TC0 for MC0 and TC6 for display/keypad | |
NVIC->ISER[0] = (0b1 << 13) | (0b1 << 19); | |
TC_0->INTENSET.bit.OVF |= 0b1; | |
TC_6->INTENSET.bit.OVF |= 0b1; | |
// Enable EIC and EXTINT for PA28 | |
NVIC->ISER[0] |= (0b1 << 4); | |
// Set priority levels | |
NVIC->IP[1] = (0b00 << 6); // EIC -- high | |
NVIC->IP[3] = (0b10 << 14); // 13 TC0 -- medium | |
NVIC->IP[4] = (0b11 << 30); // 19 TC6 -- low | |
} | |
void init_adc(void) | |
{ | |
ADC->REFCTRL.reg |= 0x2; // Set Reference Voltage | |
ADC->SAMPCTRL.reg |= 0x0; // Sample Time | |
ADC->CTRLB.bit.DIFFMODE = 0x0; // Single Ended | |
ADC->CTRLB.bit.RESSEL = 0x0; // 12-bit | |
ADC->CTRLB.bit.PRESCALER |= 0x7; // div512 | |
ADC->INPUTCTRL.bit.GAIN |= 0xF; // Set Gain (1/2) (SET GAIN (bits 24-27) HIGH) | |
// Mux Select | |
ADC->INPUTCTRL.bit.MUXPOS |= 0x13; // Ain[19] | |
ADC->INPUTCTRL.bit.MUXNEG |= 0x18; // GND | |
// Enable ADC | |
ADC->CTRLA.reg = (1u << 1); | |
} | |
int read_adc(void) | |
{ | |
// start the conversion | |
ADC->SWTRIG.reg |= 0x2; | |
// bit 0 is RESRDY (means conversion result is available) | |
while(!(ADC->INTFLAG.bit.RESRDY)); //wait for conversion to be available | |
// Return register where ADC stores result | |
return((unsigned int) ADC->RESULT.reg); | |
} | |
void init_EIC(void) | |
{ | |
EIC->INTENSET.reg |= (0b1 << 8); | |
EIC->CONFIG[1].reg |= (0x3); //PA28 | |
EIC->CTRL.reg |= (0b1 << 1); | |
} | |
void init_TC(void) | |
{ | |
// TC4 | |
TC_4->CTRLA.bit.WAVEGEN |= 0x2; // Set waveform gen to Match PWM | |
TC_4->CTRLA.bit.MODE |= 0x1; // Set counter to 8-bit mode | |
while(TC_4->STATUS.bit.SYNCBUSY); // Check for Synchronization | |
TC_4->PER.reg = 100; | |
while(TC_4->STATUS.bit.SYNCBUSY); // Check for Synchronization | |
TC_4->CC[1].reg = 50; | |
TC_4->CC[0].reg = 50; | |
while(TC_4->STATUS.bit.SYNCBUSY); // Check for Synchronization | |
TC_4->CTRLC.bit.CPTEN1 = 0x1; // Enable CC[1] | |
while(TC_4->STATUS.bit.SYNCBUSY); | |
TC_4->CTRLC.bit.CPTEN0 = 0x1; // Enable CC[0] | |
/*Enable TC*/ | |
TC_4->CTRLA.reg |= (1u << 1); | |
// Check for Synchronization | |
while(TC_4->STATUS.bit.SYNCBUSY); | |
} | |
void array_conversion(int number) | |
{ | |
num[0] = (char) ((number % 10) + '0'); | |
number /= 10; | |
num[1] = (char) ((number % 10) + '0'); | |
number /= 10; | |
num[2] = (char) ((number % 10) + '0'); | |
number /= 10; | |
num[3] = (char) (number + '0'); | |
} | |
void display() | |
{ | |
// Digit A (PA_07) | |
porA->OUTCLR.reg = 0x80; | |
SSD_display(num[3]); | |
wait(1); | |
porB->OUTSET.reg = 0xFF; | |
porA->OUTSET.reg = 0x80; | |
// Digit B | |
porA->OUTCLR.reg = 0x40; | |
SSD_display(num[2]); | |
wait(1); | |
porB->OUTSET.reg = 0xFF; | |
porA->OUTSET.reg = 0x40; | |
// Digit C | |
porA->OUTCLR.reg = 0x20; | |
SSD_display(num[1]); | |
wait(1); | |
porB->OUTSET.reg = 0xFF; | |
porA->OUTSET.reg = 0x20; | |
// Digit D (PA_04) | |
porA->OUTCLR.reg = 0x10; | |
SSD_display(num[0]); | |
wait(1); | |
porB->OUTSET.reg = 0xFF; | |
porA->OUTSET.reg = 0x10; | |
} | |
//Selects which digit to display based on row/column | |
void SSD_display(char digit) | |
{ | |
// Turn all segments off (high) | |
porB->OUTSET.reg = 0xFF; | |
switch(digit) { | |
case '9': porB->OUTCLR.reg = 0x67; break; //9 | |
case '8': porB->OUTCLR.reg = 0x7F; break; //8 | |
case '7': porB->OUTCLR.reg = 0x07; break; //7 | |
case '6': porB->OUTCLR.reg = 0x7D; break; //6 | |
case '5': porB->OUTCLR.reg = 0x6D; break; //5 | |
case '4': porB->OUTCLR.reg = 0x66; break; //4 | |
case '3': porB->OUTCLR.reg = 0x4F; break; //3 | |
case '2': porB->OUTCLR.reg = 0x5B; break; //2 | |
case '1': porB->OUTCLR.reg = 0x06; break; //1 | |
case '0': porB->OUTCLR.reg = 0x3F; break; //0 | |
default: porB->OUTCLR.reg = 0x40; break; //- | |
} | |
} | |
//time delay function | |
void wait(int t) | |
{ | |
count = 0; | |
while (count < t*1000) | |
{ | |
count++; | |
} | |
} | |
//Simple Clock Initialization | |
void Simple_Clock_Init(void) | |
{ | |
/* Various bits in the INTFLAG register can be set to one at startup. | |
This will ensure that these bits are cleared */ | |
SYSCTRL->INTFLAG.reg = SYSCTRL_INTFLAG_BOD33RDY | SYSCTRL_INTFLAG_BOD33DET | | |
SYSCTRL_INTFLAG_DFLLRDY; | |
system_flash_set_waitstates(0); //Clock_flash wait state =0 | |
SYSCTRL_OSC8M_Type temp = SYSCTRL->OSC8M; /* for OSC8M initialization */ | |
temp.bit.PRESC = 0; // no divide, i.e., set clock=8Mhz (see page 170) | |
temp.bit.ONDEMAND = 1; // On-demand is true | |
temp.bit.RUNSTDBY = 0; // Standby is false | |
SYSCTRL->OSC8M = temp; | |
SYSCTRL->OSC8M.reg |= 0x1u << 1; // SYSCTRL_OSC8M_ENABLE bit = bit-1 (page 170) | |
PM->CPUSEL.reg = (uint32_t)0; // CPU and BUS clocks Divide by 1 (see page 110) | |
PM->APBASEL.reg = (uint32_t)0; // APBA clock 0= Divide by 1 (see page 110) | |
PM->APBBSEL.reg = (uint32_t)0; // APBB clock 0= Divide by 1 (see page 110) | |
PM->APBCSEL.reg = (uint32_t)0; // APBB clock 0= Divide by 1 (see page 110) | |
PM->APBAMASK.reg |= 01u<<3; // Enable Generic clock controller clock (page 127) | |
/* Software reset Generic clock to ensure it is re-initialized correctly */ | |
GCLK->CTRL.reg = 0x1u << 0; // Reset gen. clock (see page 94) | |
while (GCLK->CTRL.reg & 0x1u ) { /* Wait for reset to complete */ } | |
// Initialization and enable generic clock #0 | |
*((uint8_t*)&GCLK->GENDIV.reg) = 0; // Select GCLK0 (page 104, Table 14-10) | |
GCLK->GENDIV.reg = 0x0100; // Divide by 1 for GCLK #0 (page 104) | |
GCLK->GENCTRL.reg = 0x030600; // GCLK#0 enable, Source=6(OSC8M), IDC=1 (page 101) | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment