From 18a89741e026cee1d5ee6b3be32e391182e8e011 Mon Sep 17 00:00:00 2001 From: Ondrej Hladuvka Date: Tue, 3 Jun 2025 11:46:39 +0300 Subject: [PATCH] project --- common/bump.c | 9 +-- common/rtos.c | 161 +++++++++++++++++++++++++++++++++++++------------ common/rtos.h | 1 + project/main.c | 65 ++++++++++++++------ 4 files changed, 174 insertions(+), 62 deletions(-) diff --git a/common/bump.c b/common/bump.c index b659c09..a553cde 100644 --- a/common/bump.c +++ b/common/bump.c @@ -3,15 +3,12 @@ #include "inc/msp432p401r.h" void BumpInit(void) { - // Port 4 pins 0, 2, 3, 5, 6, 7 - // and enables internal resistors P4->SEL0 &= ~BUMP_PINS; - P4->SEL1 &= ~BUMP_PINS; // 1) configure P4.0 P4.2 P4.3 P4.5 P4.6 P4.7 as gpio - P4->DIR &= ~BUMP_PINS; // 2) configure as input - P4->REN |= BUMP_PINS; // 3) enable pull resisors + P4->SEL1 &= ~BUMP_PINS; // configure P4.0 P4.2 P4.3 P4.5 P4.6 P4.7 as gpio + P4->DIR &= ~BUMP_PINS; // configure as input + P4->REN |= BUMP_PINS; // enable pull resisors } uint8_t BumpRead(void) { - // write this as part of Lab 2 return P4->IN; } \ No newline at end of file diff --git a/common/rtos.c b/common/rtos.c index 0668e65..24be62c 100644 --- a/common/rtos.c +++ b/common/rtos.c @@ -10,51 +10,86 @@ #define THREADS 2 #define PERIODIC_THREADS 2 -#define SIZE 100 +#define STACKSIZE 100 typedef struct Tcb { - int32_t *sp; // stack pointer - struct Tcb *next; // linked-list - int32_t *blocked; // shows on which semaphore it is blocked + int32_t* sp; // stack pointer + struct Tcb* next; // linked-list + int32_t* blocked; // shows on which semaphore it is blocked + int32_t sleep; } Tcb_t; PeriodicEvent_t periodic_event_threads[PERIODIC_THREADS]; -Tcb_t tcbs[THREADS]; -Tcb_t *run_pt; -int32_t stacks[THREADS][SIZE]; +Tcb_t tcbs[THREADS]; +Tcb_t* run_pt; +int32_t stacks[THREADS][STACKSIZE]; void RunPeriodicEvents(void); -void __attribute__((naked)) -SysTick_Handler(void) { // 1) Saves R0-R3,R12,LR,PC,PSR - __asm( // 2) Prevent interrupt during switch - // 3) Save remaining regs r4-11 - // 4) R0=pointer to run_pt, old thread - // 5) Save SP into TCB - // 6) R1 = run_pt, new thread - // 7) new thread SP; SP = run_pt->sp; - // 8) restore regs r4-11 - // 9) tasks run with interrupts enabled - ); // 10) restore R0-R3,R12,LR,PC,PSR +void __attribute__((naked)) SysTick_Handler(void) { + __asm volatile ( + "CPSID I \n" // disable interrupts + "PUSH {R4-R11} \n" // save R4-R11 + "LDR R0, =run_pt \n" // R0 = pointer to run_pt + + "LDR R1, [R0] \n" // R1 = run_pt + "STR SP, [R1] \n" // save current SP + + "PUSH {R0, LR} \n" // save R0, LR + "BL Scheduler \n" // call scheduler + "POP {R0, LR} \n" // restore R0, LR + + "LDR R1, [R0] \n" // R1 = run_pt + "LDR SP, [R1] \n" // restore new SP + + "POP {R4-R11} \n" // restore R4-R11 + "CPSIE I \n" // enable interrupts + "BX LR \n" // exit exception + ".align 4 \n" + ); } void __attribute__((naked)) StartOS(void) { - __asm(// currently running thread - // R2 = value of run_pt - // new thread SP; SP = run_pt->sp; - // restore regs r4-11 - // restore regs r0-3 - // discard LR from initial stack - // start location - // discard PSR - // Enable interrupts at processor level - ); // start first thread + __asm volatile ( + "LDR R0, =run_pt \n" // address of run_pt + "LDR R1, [R0] \n" // R1 = run_pt + "LDR SP, [R1] \n" // SP = run_pt->sp + + "POP {R4-R11} \n" // restore R4-R11 + "POP {R0-R3} \n" // restore R0-R3 + "POP {R12} \n" // restore R12 + "ADD SP, SP, #4 \n" // skip LR + "POP {LR} \n" // load thread start address + "ADD SP, SP, #4 \n" // skip PSR + + "CPSIE I \n" // enable interrupts + "BX LR \n" // start thread + ".align 4 \n" + ); } -void SetInitialStack(int idx) { - // Write this code + +void SetInitialStack(int i) { + tcbs[i].sp = &stacks[i][STACKSIZE-16]; + stacks[i][STACKSIZE-1] = 0x01000000; // xPSR thumb bit + + stacks[i][STACKSIZE-16] = 16; // R4 + stacks[i][STACKSIZE-15] = 15; // R5 + stacks[i][STACKSIZE-14] = 14; // R6 + stacks[i][STACKSIZE-13] = 13; // R7 + stacks[i][STACKSIZE-12] = 12; // R8 + stacks[i][STACKSIZE-11] = 11; // R9 + stacks[i][STACKSIZE-10] = 10; // R10 + stacks[i][STACKSIZE-9] = 9; // R11 + stacks[i][STACKSIZE-8] = 8; // R0 + stacks[i][STACKSIZE-7] = 7; // R1 + stacks[i][STACKSIZE-6] = 6; // R2 + stacks[i][STACKSIZE-5] = 5; // R3 + stacks[i][STACKSIZE-4] = 4; // R12 + stacks[i][STACKSIZE-3] = 0; // LR (0 causes fault if thread returns) + } // Initialize operating system, disable interrupts @@ -66,11 +101,23 @@ void RoundRobinInit(void) { } void AddThreads(void (**threads)(void)) { - // Write this code + uint32_t i = 0; + for (; i < THREADS; ++i) { + SetInitialStack(i); + stacks[i][STACKSIZE-2] = (int32_t)threads[i]; + tcbs[i].next = &tcbs[ (i+1) % THREADS ]; + tcbs[i].blocked = 0; + tcbs[i].sleep = 0; + // tcbs[i].priority = p0; + } + run_pt = &tcbs[0]; } void AddPeriodicEventThreads(PeriodicEvent_t *periodic_events) { - // Write this code + uint32_t i = 0; + for (;i < PERIODIC_THREADS; ++i) { + periodic_event_threads[i] = periodic_events[i]; + } } void RoundRobinLaunch(uint32_t time_slice_cycles) { @@ -79,28 +126,66 @@ void RoundRobinLaunch(uint32_t time_slice_cycles) { SysTick->VAL = 0; // 3) any write to current clears it SCB->SHP[11] = 7 << 5; // set priority into top 3 bits of 8-bit register SysTick->CTRL = 0x00000007; // 4) enable SysTick with core clock and interrupts + + TimerA1Init(RunPeriodicEvents, 5000); // 100Hz StartOS(); // start on the first task } void RunPeriodicEvents(void) { - // Write this code (no sleep decreasing) + int32_t i; + for (i = 0; i < THREADS; ++i) { + if (tcbs[i].sleep) { + --(tcbs[i].sleep); + } + } + for (i = 0; i < PERIODIC_THREADS; ++i) { + periodic_event_threads[i].event(); + } } void Scheduler(void) { - // Write this code (no sleep check) + Tcb_t* t = run_pt->next; + while (t->blocked || t->sleep) { + t = t->next; + } + + run_pt = t; } void Suspend(void) { - SysTick->VAL = 0; // any write to current clears it - SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk; // trigger SysTick + SysTick->VAL = 0; // clear + SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk; // trigger +} + +void Sleep(int32_t time) { + run_pt->sleep = time; + Suspend(); } // Decrement semaphore void Wait(int32_t *sema_pt) { - // Write this code similar to RTOS3 + CPU_cpsid(); + --(*sema_pt); + if((*sema_pt) < 0) { + run_pt->blocked = sema_pt; // reason it is blocked + + CPU_cpsie(); + Suspend(); + } + CPU_cpsie(); } // Increment semaphore void Signal(int32_t *sema_pt) { - // Write this code similar to RTOS3 + Tcb_t *pt; + CPU_cpsid(); + ++(*sema_pt); + if ((*sema_pt) <= 0) { + pt = run_pt->next; + while(pt->blocked != sema_pt) { + pt = pt->next; + } + pt->blocked = 0; + } + CPU_cpsie(); } \ No newline at end of file diff --git a/common/rtos.h b/common/rtos.h index 938e917..61a7d5f 100644 --- a/common/rtos.h +++ b/common/rtos.h @@ -12,6 +12,7 @@ void RoundRobinInit(void); void AddThreads(void (**threads)(void)); void AddPeriodicEventThreads(PeriodicEvent_t *periodic_events); void RoundRobinLaunch(uint32_t time_slice_cycles); +void Sleep(int32_t time); void Wait(int32_t *sema_pt); void Signal(int32_t *sema_pt); diff --git a/project/main.c b/project/main.c index c2bc3b6..e0a6b21 100644 --- a/project/main.c +++ b/project/main.c @@ -2,12 +2,12 @@ #include #include -#include "rtos.h" -#include "pwm.h" -#include "bump.h" -#include "reflectance.h" -#include "motor.h" -#include "delay.h" +#include "../common/rtos.h" +#include "../common/pwm.h" +#include "../common/bump.h" +#include "../common/reflectance.h" +#include "../common/motor.h" +#include "../common/delay.h" #include "inc/msp432p401r.h" @@ -29,6 +29,11 @@ int32_t bump_pressed; // semaphore uint8_t bump_data, reflectance_data; +void WaitTouchRelease(void) { + while(!(SW1IN || SW2IN)); // wait for touch + while( SW1IN || SW2IN ); // wait for release +} + void SwitchInit(void) { P1->SEL0 &= ~0x12; P1->SEL1 &= ~0x12; // 1) configure P1.4 and P1.1 as GPIO @@ -51,7 +56,7 @@ void ColorLEDInit(void) { P2->OUT &= ~0x07; // all LEDs off } -void Task1(void) { +void led_task(void) { while (true) { if (SW1IN && SW2IN) { GREENOUT = 1; @@ -76,32 +81,56 @@ void Task1(void) { } } -void Task2(void) { - while (true) { - // Motor goes forward and waits until bump is pressed - // When pressed, go backward for some time and turn left for some time +void drive_task(void) { + while (1) { + bump_pressed = 0; + MotorForward(1000, 1000); + Wait(&bump_pressed); + MotorBackward(800, 800); + Sleep(1000); + MotorLeft(800, 800); + Sleep(2000); } } -void Task3(void) { - // Read reflectance similar to Lab 3 +uint32_t refl_tick_count = 0; +void reflectance_task(void) { + ++refl_tick_count; + + if (1 == (refl_tick_count % 10)) { + ReflectanceStart(); + } + + if (2 == (refl_tick_count % 10)) { + reflectance_data = ReflectanceEnd(); + } } -void Task4(void) { - // Read bump and signal if any pressed +void bump_task(void) { + bump_data = BumpRead(); + if ((~bump_data & BUMP_PINS)) { + Signal(&bump_pressed); + } } int main(void) { BumpInit(); ReflectanceInit(); - PWMInitMotor(?); + PWMInitMotor(3751); MotorInit(); + RedLEDInit(); ColorLEDInit(); SwitchInit(); RoundRobinInit(); - AddThreads((void (*[])(void)){Task1, Task2}); - AddPeriodicEventThreads((PeriodicEvent_t []){{Task3, 1}, {Task4, 100}}); + AddThreads((void (*[])(void)){ + led_task, + drive_task + }); + AddPeriodicEventThreads((PeriodicEvent_t []) { + {reflectance_task, 1}, + {bump_task, 100} + }); RoundRobinLaunch(48000); // 1ms return 0; // never executes } \ No newline at end of file