RTOS lab using FreeRTOS 8.0.1 for Atmel studios for a SAMD20J and pre-made PCB board for hardware interfacing. Operating System made from scratch to utilize a better timing management instead of the basic programming. RTOS was utilized to control a PID system for a motor to move at 1500 RPM on command by user input from a keypad. On a separate c…

RTOS,Lab7,EE138

//***************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
	~
*/

#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)
}