adipu/TweinStein
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
TweinStein/RTOS_SensorBoard/RTOS_SensorBoard.c

1390 lines
48 KiB
C
Raw Permalink Normal View History

all
2026-06-12 02:55:04 -07:00
/* RTOS_Lab4.c
* Jonathan Valvano
* December 30, 2025
* Remove 3.3V J101 jumper to run RTOS sensor board or motor board
* A two-pin female header is required on the LaunchPad TP10(XDS_VCC) and TP9(!RSTN)
*/
// Overtake Parameters
#define FRONT_DETECT 1200
#define D2_MIN_PASS 250
#define LD2_MIN_PASS 250
#define V_MAX_MMPS 1140 // TBD via calibration
#define V_ALPHA_Q8 77
#define R_ACC_ALPHA_Q8 205
#define R_THRESH_BASE 150
#define R_YAW_GAIN_Q8 77
#define R_CAP 2000
#define OFFSET_CMD 180
#define OFFSET_RATE 20
#define GZ_RAMP_GATE 500
#define PASS_CLEAR 900
#define T_CLEAR_MS 500
#define T_PASS_MIN 400
#define T_SETTLE 300
#define ABORT_SIDE 150
#define ABORT_GZ 1200
#define ABORT_DF 300 // guessed value
#define CALIB_THROTTLE 9500
// PD weights
#define kp_d 1
#define kd_d 2
#define kp_a 5
#define kd_a 2
#include <ti/devices/msp/msp.h>
#include "../inc/LaunchPad.h"
#include "../RTOS_Labs_common/ADC.h"
#include "../inc/Clock.h"
#include "../RTOS_Labs_common/ST7735_SDC.h"
#include "../RTOS_Labs_common/RTOS_UART.h"
#include "../RTOS_Labs_common/Interpreter.h"
#include "../RTOS_Labs_common/IRDistance.h"
#include "../RTOS_Labs_common/LPF.h"
#include "../RTOS_Labs_common/DFT16.h"
#include "../RTOS_Labs_common/TFLuna1.h"
#include "../RTOS_Labs_common/TFLuna2.h"
#include "../RTOS_Labs_common/TFLuna3.h"
#include "../RTOS_Labs_common/OS.h"
#include "../RTOS_Labs_common/eDisk.h"
#include "../RTOS_Labs_common/eFile.h"
#include "../RTOS_Labs_common/CAN.h"
#include "Model.h"
#include "IMU.h"
#include <stdio.h>
#include <stdlib.h>
#define USE_MEDIAN_FILTER 0 // 1 = median (Median5 per sample, 5-sample pace), 0 = mean (8-sample average)
#define angle_ref 5 // degrees "0"
int32_t dist_ref_cur = 0; // mm
// PA10 is UART0 Tx index 20 in IOMUX PINCM table
// PA11 is UART0 Rx index 21 in IOMUX PINCM table
// Insert jumper J25: Connects PA10 to XDS_UART
// Insert jumper J26: Connects PA11 to XDS_UART
// PA0 is red LED1, index 0 in IOMUX PINCM table, negative logic
// PB22 is BLUE LED2, index 49 in IOMUX PINCM table
// PB26 is RED LED2, index 56 in IOMUX PINCM table
// PB27 is GREEN LED2, index 57 in IOMUX PINCM table
// PA18 is S1 positive logic switch, conflict with TFLuna1, so S1 will not be used
// PB21 is S2 negative logic switch, used for aperiodic task
// IR analog distance sensors
// 30 cm GP2Y0A41SK0F or 80 cm long range GP2Y0A21YK0F
// PA26 Right ADC0_1
// PA24 Center ADC0_3, used in Labs 1,2,3,4
// PA22 Left ADC0_7
// PA27 Extra ADC0_0
// RTOS sensor board supported three TF-Luna sensors
// Serial TxD: PA17 is UART1 Tx (MSPM0 to TFLuna1)
// Serial RxD: PA18 is UART1 Rx (TFLuna1 to MSPM0), conflict with LaunchPad S1
// Serial TxD: PB17 is UART2 Tx (MSPM0 to TFLuna2), used in Labs 1,2,3,4
// Serial RxD: PB18 is UART2 Rx (TFLuna2 to MSPM0), used in Labs 1,2,3,4
// Serial TxD: PB12 is UART3 Tx (MSPM0 to TFLuna3),
// Serial RxD: PB13 is UART3 Rx (TFLuna3 to MSPM0), shared with LD19 Lidar
//UART3 is shared between LD19 and TFLuna3 (can have either but not both)
// **** OS must run disk_timerproc(); at 1000Hz, every 1ms *****
long StartCritical(void);
void EndCritical(long sr);
uint32_t Running; // true while robot is running
uint32_t NumCreated; // number of foreground threads created
//---------------------User debugging-----------------------
// Unused sensor board pins, made outputs for debugging
// Jumper J14 select PB23
// Jumper J15 select PA16
void Logic_Init(void){
IOMUX->SECCFG.PINCM[PA8INDEX] = (uint32_t) 0x00000081;
IOMUX->SECCFG.PINCM[PB23INDEX] = (uint32_t) 0x00000081; //****CHANGE from PA9****
IOMUX->SECCFG.PINCM[PA16INDEX] = (uint32_t) 0x00000081;
IOMUX->SECCFG.PINCM[PB4INDEX] = (uint32_t) 0x00000081;
IOMUX->SECCFG.PINCM[PB1INDEX] = (uint32_t) 0x00000081;
IOMUX->SECCFG.PINCM[PB20INDEX] = (uint32_t) 0x00000081;
GPIOA->DOE31_0 |= (1<<8)|(1<<16); //****CHANGE removing PA9****
GPIOB->DOE31_0 |= (1<<4)|(1<<1)|(1<<20)|(1<<23);//****CHANGE adding PB23****
}
#define TogglePA8() (GPIOA->DOUTTGL31_0 = (1<<8))
#define SetPA8() (GPIOA->DOUTSET31_0 = (1<<8))
#define ClrPA8() (GPIOA->DOUTCLR31_0 = (1<<8))
#define TogglePB23() (GPIOB->DOUTTGL31_0 = (1<<23))
#define SetPB23() (GPIOB->DOUTSET31_0 = (1<<23))
#define ClrPB23() (GPIOB->DOUTCLR31_0 = (1<<23))
#define TogglePA16() (GPIOA->DOUTTGL31_0 = (1<<16))
#define TogglePB4() (GPIOB->DOUTTGL31_0 = (1<<4))
#define SetPB4() (GPIOB->DOUTSET31_0 = (1<<4))
#define ClrPB4() (GPIOB->DOUTCLR31_0 = (1<<4))
#define TogglePB1() (GPIOB->DOUTTGL31_0 = (1<<1))
#define TogglePB20() (GPIOB->DOUTTGL31_0 = (1<<20))
//--------------Arctan Lookup Table-----------------------
// TanTable[i] = tan(i degrees) * 1000, for i = 0 to 89
const uint32_t TanTable[90] = {
0, 17, 35, 52, 70, 87, 105, 123, 141, 158, // 0-9
176, 194, 213, 231, 249, 268, 287, 306, 325, 344, // 10-19
364, 384, 404, 424, 445, 466, 488, 510, 532, 554, // 20-29
577, 601, 625, 649, 675, 700, 727, 754, 781, 810, // 30-39
839, 869, 900, 933, 966, 1000, 1036, 1072, 1111, 1150, // 40-49
1192, 1235, 1280, 1327, 1376, 1428, 1483, 1540, 1600, 1664, // 50-59
1732, 1804, 1881, 1963, 2050, 2145, 2246, 2356, 2475, 2605, // 60-69
2747, 2904, 3078, 3271, 3487, 3732, 4011, 4331, 4705, 5145, // 70-79
5671, 6314, 7115, 8144, 9514,11430,14300,19081,28636,57290 // 80-89
};
// arctan: inverse tangent using table lookup and binary search
// Input: ratio * 1000 (e.g., 1000 for ratio=1.0, -500 for ratio=-0.5)
// Output: angle in degrees (-89 to 89)
// Usage: angle = arctan((opposite * 1000) / adjacent);
int32_t arctan(int32_t ratio_x1000) {
uint32_t abs_ratio = (ratio_x1000 < 0) ? (uint32_t)(-ratio_x1000) : (uint32_t)ratio_x1000;
uint32_t lo = 0, hi = 88;
if (abs_ratio >= TanTable[89]) {
return (ratio_x1000 < 0) ? -89 : 89;
}
while (lo + 1 < hi) {
uint32_t mid = (lo + hi) / 2;
if (TanTable[mid] <= abs_ratio) lo = mid;
else hi = mid;
}
return (ratio_x1000 < 0) ? -(int32_t)lo : (int32_t)lo;
}
// tangent: tangent of angle using TanTable (same table as arctan)
// Input: angle in degrees
// Output: tan(angle) * 1000; returns 57290 (max) for angles near 90/270 (undefined)
// tan has period 180: sign positive in [0,89] and [180,269], negative in [91,179] and [271,359]
int32_t tangent(int32_t angle_deg) {
while (angle_deg < 0) angle_deg += 360;
while (angle_deg >= 360) angle_deg -= 360;
uint32_t mod = (uint32_t)angle_deg % 180;
if (mod <= 89) return (int32_t)TanTable[mod];
if (mod == 90) return 57290; // clamp — approaches +∞ before, -∞ after
return -(int32_t)TanTable[180 - mod]; // mod 91–179: negative, use symmetry
}
//-----------end of Arctan Lookup Table--------------------
//--------------Cosine Lookup Table-----------------------
// CosTable[i] = cos(i degrees) * 1000, for i = 0 to 90
const uint32_t CosTable[91] = {
1000, 999, 999, 998, 997, 996, 994, 992, 990, 987, // 0-9
984, 981, 978, 974, 970, 965, 961, 956, 951, 945, // 10-19
939, 933, 927, 920, 913, 906, 898, 891, 882, 874, // 20-29
866, 857, 848, 838, 829, 819, 809, 798, 788, 777, // 30-39
766, 754, 743, 731, 719, 707, 694, 681, 669, 656, // 40-49
642, 629, 615, 601, 587, 573, 559, 544, 529, 515, // 50-59
500, 484, 469, 453, 438, 422, 406, 390, 374, 358, // 60-69
342, 325, 309, 292, 276, 258, 242, 224, 208, 190, // 70-79
173, 156, 139, 121, 104, 87, 69, 52, 35, 17, // 80-89
0 // 90
};
// cosine: cosine of angle using table lookup
// Input: angle in degrees (-180 to 180)
// Output: cos(angle) * 1000 (e.g., 1000 for 0 deg, 0 for 90 deg, -1000 for 180 deg)
int32_t cosine(int32_t angle_deg) {
// Normalize to [0, 360)
while (angle_deg < 0) angle_deg += 360;
while (angle_deg >= 360) angle_deg -= 360;
// Use symmetry: cos is even and periodic
if (angle_deg <= 90) return (int32_t)CosTable[angle_deg];
if (angle_deg <= 180) return -(int32_t)CosTable[180 - angle_deg];
if (angle_deg <= 270) return -(int32_t)CosTable[angle_deg - 180];
return (int32_t)CosTable[360 - angle_deg];
}
//-----------end of Cosine Lookup Table--------------------
uint32_t Checks; // number of times virus checking has run
uint32_t ChecksWork; // number of checks in 10 second
//------------------Task 1--------------------------------
// real-time sampling ADC0 channel 3, using software start trigger
// 60-Hz notch high-Q, IIR filter, assuming fs=1000 Hz
// y(n) = (256x(n) -476x(n-1) + 256x(n-2) + 471y(n-1)-251y(n-2))/256 (1k sampling)
#define PERIOD TIME_1MS // DAS 1kHz sampling period in system time units
#define FS 1000 // DAS sampling
#define RUNLENGTH (10000) // display results and quit when FilterWork==RUNLENGTH
uint32_t FilterOutput, Distance, DistanceRaw;
uint32_t L_FilterOutput, L_Distance, L_DistanceRaw;
uint32_t L_Distance2;
uint32_t FrontDist = 1000; // mm, from TFLuna1 (front-facing); default far
Sema4_t LCDFree; // SDC and LCD sharing
uint32_t FilterWork;
uint32_t MaxJitter3;
#define JITTERSIZE3 512
uint32_t const JitterSize3=JITTERSIZE3;
uint32_t JitterHistogram3[JITTERSIZE3]={0,};
void Jitter3_Init(void){
for(int i=0;i<JitterSize3;i++){
JitterHistogram3[i] = 0;
}
MaxJitter3 = 0;
}
//******** DAS ***************
// background thread, calculates 60Hz notch filter
// runs 1000 times/sec
// samples PA24 Center ADC0_3, calculates Distance
// inputs: none
// outputs: none
void DAS(void){
uint32_t input, L_input;
static uint32_t LastTime; // time at previous ADC sample, 12.5 ns
uint32_t thisTime; // time at current ADC sample, 12.5 ns
uint32_t jitter; // time between measured and expected, 12.5 ns
TogglePA8(); // toggle PA8
ADC_InDual(ADC0, &input, &L_input); // ch1=right IR (PA26), ch7=left IR (PA22)
TogglePA8(); // toggle PA8
thisTime = OS_Time(); // current time, 12.5 ns
FilterOutput = Filter(input);
DistanceRaw = FilterOutput;
Distance = IRDistance_Right(FilterOutput); // in mm
L_FilterOutput = Filter2(L_input);
L_DistanceRaw = L_FilterOutput;
L_Distance = IRDistance_Left(L_FilterOutput); // in mm
if(Running){ // finite time run
FilterWork++; // calculation finished
if(FilterWork>2){ // ignore timing of first interrupt
uint32_t diff = OS_TimeDifference(LastTime,thisTime);
if(diff>PERIOD){
jitter = (diff-PERIOD); // in 12.5 ns
}else{
jitter = (PERIOD-diff); // in 12.5 ns
}
if(jitter > MaxJitter3){
MaxJitter3 = jitter; // in 12.5 ns
} // jitter should be 0
if (jitter > JITTERSIZE3-1) jitter = JITTERSIZE3-1; // clamp
JitterHistogram3[jitter]++;
}
ChecksWork = Checks;
LastTime = thisTime;
}
TogglePA8(); // toggle PA8
}
//--------------end of Task 1-----------------------------
//------------------Task 2--------------------------------
// background thread executes with PA28 button
// PA28 negative logic switch
// one foreground task created with each button push
// foreground tread outputs a message and dies
uint32_t DataLost; // data sent by Producer, but not received by Consumer
// ***********PA28Push*************
int ArmCrash=1;
void HandleCrash(void){
TogglePB23();
TogglePB23();
uint32_t myId = OS_Id();
ST7735_Message(1,0,"myID =",myId);
ST7735_Message(1,1,"*Crash*, t= ",OS_MsTime());
ArmCrash=1;
TogglePB23();
OS_Kill();
}
void PA28Push(void){ // real time task
if(ArmCrash){
ArmCrash = 0; // debounce
NumCreated += OS_AddThread(&HandleCrash,128,1); // test robot crash
}
}
//------------------Task 3--------------------------------
// hardware-triggered TFLuna distance sampling at 100Hz
// Producer runs as part of UART2 ISR
// Producer uses fifo to transmit 100 distance samples/sec to Consumer
// every 64 samples, Consumer calculates FFT
// every 2.5ms*64 = 160 ms (6.25 Hz), consumer sends data to Display via mailbox
// Display thread updates LCD with measurement
uint32_t DataLost; // data sent by Producer, but not received by Consumer
uint32_t Distance2; // mm
int32_t x[16],ReX[16],ImX[16]; // input and output arrays for FFT
Sema4_t TFLuna3Ready; // signaled by Producer (right, TFLuna3) after global update
Sema4_t TFLuna2Ready; // signaled by Producer2 (left, TFLuna2) after global update
//******** Producer ***************
// The Producer in this lab will be called from the UART2 ISR
// The TFLuna2 samples distance at about 100 Hz
// sends data to the consumer, runs periodically at 100Hz
void Producer(uint32_t data){
if(Running){ // finite time run
TogglePA16(); // toggle PA16
#if USE_MEDIAN_FILTER
Distance2 = (uint32_t)Median5((int32_t)data);
#else
Distance2 = data;
#endif
OS_bSignal(&TFLuna3Ready);
TogglePA16(); // toggle PA16
}
}
void Producer2(uint32_t data){
if(Running){
#if USE_MEDIAN_FILTER
L_Distance2 = (uint32_t)Median5_2((int32_t)data);
#else
L_Distance2 = data;
#endif
OS_bSignal(&TFLuna2Ready);
}
}
uint32_t FrontCount = 0; // debug: increments each time TFLuna1 ISR delivers data
void Producer3(uint32_t data){
#if USE_MEDIAN_FILTER
FrontDist = (uint32_t)Median((int32_t)data);
#else
FrontDist = data;
#endif
FrontCount++;
}
void Display(void);
// Describe the error with text on the LCD and then stall.
// If you are getting disk errors, rerun Testmain1 Testmain2 Testmain3
void diskError(char *errtype, int32_t code){
OS_bSignal(&LCDFree);
ST7735_DrawString(0, 1, "Err: ", ST7735_RED);
ST7735_DrawString(5, 1, errtype, ST7735_RED);
ST7735_DrawString(0, 2, "Code: ", ST7735_RED);
ST7735_SetCursor(6, 2);
ST7735_SetTextColor(ST7735_RED);
ST7735_OutUDec(code);
Running = 0;
OS_Kill();
}
void StartFileDump(char *pt){
OS_bWait(&LCDFree);
eFile_Create(pt); // ignore error if file already exists
if(eFile_WOpen(pt)) diskError("eFile_WOpen",0);
if(eFile_WriteString("time_ms,elapsed,tf_r,tf_l,tf_front,sp,ot_state\n")) diskError("eFile_WriteString",0);
OS_bSignal(&LCDFree);
}
void EndFileDump(){
OS_bWait(&LCDFree);
if(eFile_WClose()) // diskError("eFile_WClose",0);
OS_bSignal(&LCDFree);
}
void FileDump(uint32_t data, uint32_t data2){
SetPB4();
OS_bWait(&LCDFree);
eFile_WriteUFix2(OS_MsTime()/10); eFile_Write('\t');
eFile_WriteUDec(data); eFile_Write('\t');
eFile_WriteUDec(data2); eFile_WriteString("\n\r");
OS_bSignal(&LCDFree);
ClrPB4();
}
#define NUMCOLS 7
// Dump row into csv
int FileDumpRow(int32_t* row){
OS_bWait(&LCDFree);
for (int i = 0; i < NUMCOLS; i++){
if (eFile_WriteSDec(row[i])){
OS_bSignal(&LCDFree);
return 1;
}
if (i < NUMCOLS - 1){
if(eFile_Write(',')){
OS_bSignal(&LCDFree);
return 1;
}
}
}
if (eFile_Write('\n')){
OS_bSignal(&LCDFree);
return 1;
}
OS_bSignal(&LCDFree);
return 0;
}
typedef enum { S_FOLLOW, S_DETECT, S_COMMIT, S_PASS, S_REJOIN } overtake_state_t;
overtake_state_t ot_state = S_FOLLOW;
int32_t ot_side = 0; // -1=right, 1=left
uint32_t ot_detect_cnt = 0;
uint32_t ot_state_entry_ms = 0, ot_clear_since_ms = 0;
int32_t front_prev_filt = 0;
int32_t dist_ref_target = 0;
int32_t v_hat = 0, r_acc = 0;
int32_t prev_throttle_avg = 0;
int32_t front_m1 = 0, front_m2 = 0;
int32_t median3_i32(int32_t a, int32_t b, int32_t c) {
if ((a <= b && b <= c) || (c <= b && b <= a)) return b;
if ((b <= a && a <= c) || (c <= a && a <= b)) return a;
return c;
}
int32_t ramp_toward(int32_t cur, int32_t tgt, int32_t step) {
if (cur < tgt) { cur += step; if (cur > tgt) cur = tgt; }
else if (cur > tgt) { cur -= step; if (cur < tgt) cur = tgt; }
return cur;
}
int32_t throttle_to_v_mmps(int32_t throttle) {
if (throttle < 0) throttle = 0;
return (int32_t)((long long)throttle * V_MAX_MMPS / 9999); // 9999 = max throttle cmd
}
overtake_state_t overtake_step(int32_t front_filt, int32_t r_acc_in, int32_t r_thresh, int32_t d2, int32_t ld2, int32_t gz_ddps, uint32_t now_ms) {
overtake_state_t next_state = ot_state;
// bail out if something wrong
if (ot_state != S_FOLLOW && ot_state != S_DETECT) {
int bail = 0;
if (front_filt < 200) bail = 1; // wall coming
if (abs(gz_ddps) > ABORT_GZ) bail = 1;
if (ot_state == S_COMMIT || ot_state == S_PASS) {
if (ot_side == -1 && d2 < ABORT_SIDE) bail = 1;
if (ot_side == 1 && ld2 < ABORT_SIDE) bail = 1;
}
int32_t df_dt = front_filt - front_prev_filt;
if (ot_state == S_COMMIT && df_dt > ABORT_DF) bail = 1; // car swerved back
if (bail) {
dist_ref_target = 0;
return S_FOLLOW;
}
}
switch(ot_state) {
case S_FOLLOW:
dist_ref_target = 0;
if (front_filt < FRONT_DETECT && r_acc_in > r_thresh && (d2 > D2_MIN_PASS || ld2 > LD2_MIN_PASS)) {
next_state = S_DETECT;
ot_detect_cnt = 0;
}
break;
case S_DETECT:
dist_ref_target = 0;
if (front_filt < FRONT_DETECT && r_acc_in > r_thresh) {
if (d2 >= D2_MIN_PASS && d2 >= ld2) { // go right
ot_side = -1;
dist_ref_target = -OFFSET_CMD;
next_state = S_COMMIT;
ot_state_entry_ms = now_ms;
// ST7735_Message(1, 4, "COMMIT R", d2);
} else if (ld2 >= LD2_MIN_PASS) { // go left
ot_side = 1;
dist_ref_target = OFFSET_CMD;
next_state = S_COMMIT;
ot_state_entry_ms = now_ms;
} else {
if (front_filt < 400) next_state = S_FOLLOW; // too close, can't pass?
}
} else {
next_state = S_FOLLOW;
}
break;
case S_COMMIT:
if (abs(dist_ref_cur - dist_ref_target) < 10) {
next_state = S_PASS;
ot_state_entry_ms = now_ms;
ot_clear_since_ms = 0;
}
break;
case S_PASS:
if (front_filt > PASS_CLEAR) {
if (ot_clear_since_ms == 0) ot_clear_since_ms = now_ms;
if ((now_ms - ot_clear_since_ms) > T_CLEAR_MS && (now_ms - ot_state_entry_ms) > T_PASS_MIN) {
next_state = S_REJOIN;
dist_ref_target = 0;
ot_state_entry_ms = now_ms;
}
} else {
ot_clear_since_ms = 0;
}
break;
case S_REJOIN:
// obstacle came back, restart passing
if (ot_side == -1 && d2 < ABORT_SIDE) {
dist_ref_target = -OFFSET_CMD;
next_state = S_PASS;
}
if (ot_side == 1 && ld2 < ABORT_SIDE) {
dist_ref_target = OFFSET_CMD;
next_state = S_PASS;
}
if (abs(dist_ref_cur) < 10 && (now_ms - ot_state_entry_ms) > T_SETTLE) next_state = S_FOLLOW;
break;
}
return next_state;
}
//******** Robot ***************
// foreground Consumer thread, accepts data from producer
// inputs: none
// outputs: none
char FileName[8]="robot0";
uint32_t elapsed = 0;
int32_t prevError = 0;
int32_t prevE_A = 0;
uint32_t prevTime = 0;
uint32_t startTime = 0;
int16_t gyroZ_bias = 0;
void Robot(void){
DataLost = 0; // new run with no lost data
FilterWork = 0;
Running = 1;
Jitter3_Init();
Model_Inputs[steering_prev] = 32768; // Initialize to 0 degrees
OS_ClearMsTime();
OS_Fifo_Init(256);
NumCreated += OS_AddThread(&Display,128,0);
UART_OutString("Robot running...");
StartFileDump(FileName);
OS_ClearMsTime();
while (LaunchPad_InS2() == 0); // REMOVE AFTER DATA COLLECTION
// Gyro-Z bias estimation. Robot must remain stationary.
UART_OutString("Calibrating IMU...");
OS_Sleep(1000);
int32_t gzSum = 0;
const uint32_t BIAS_SAMPLES = 64; // ~1.6 s at 25 ms spacing
for (uint32_t i = 0; i < BIAS_SAMPLES; i++) {
long csr = StartCritical();
int16_t g = IMU_GyroZ;
EndCritical(csr);
gzSum += g;
OS_Sleep(25); // wait for IMU update
}
gyroZ_bias = (int16_t)(gzSum / (int32_t)BIAS_SAMPLES);
UART_OutString(" gz_bias=");
UART_OutSDec(gyroZ_bias);
UART_OutString("\n\r");
startTime = OS_MsTime();
while(1) {
elapsed = OS_MsTime() - prevTime;
prevTime = OS_MsTime();
#if USE_MEDIAN_FILTER
for (uint8_t i = 0; i < 5; i++) {
OS_bWait(&TFLuna3Ready);
OS_bWait(&TFLuna2Ready);
}
__disable_irq();
uint32_t d2 = Distance2;
uint32_t ld2 = L_Distance2;
__enable_irq();
#else
uint32_t d2 = 0;
uint32_t ld2 = 0;
for (uint8_t i = 0; i < 8; i++) {
OS_bWait(&TFLuna3Ready); // block until right-side TFLuna has fresh data
OS_bWait(&TFLuna2Ready); // block until left-side TFLuna has fresh data
__disable_irq();
d2 += Distance2;
ld2 += L_Distance2;
__enable_irq();
}
d2 >>= 3;
ld2 >>= 3;
#endif
__disable_irq();
uint32_t d_ir = Distance;
uint32_t ld_ir = L_Distance;
__enable_irq();
// FOR CALIBRATION
// ST7735_Message(1, 0, "L_DistanceRaw: ", L_DistanceRaw);
// ST7735_Message(1, 1, "DistanceRaw: ", DistanceRaw);
// IR correction: if TFLuna sees open space (>600mm) and reads significantly
// more than the IR, the IR is past its calibrated range — clamp to 305mm.
if (d2 > 600 && d2 > d_ir + 150) d_ir = 305;
if (ld2 > 600 && ld2 > ld_ir + 150) ld_ir = 305;
// Read IMU to use for controller refinement
long sr = StartCritical();
int16_t gz_raw = IMU_GyroZ;
int16_t ax = IMU_AccelX;
int16_t ay = IMU_AccelY;
EndCritical(sr);
int16_t gz = (int16_t)(gz_raw - gyroZ_bias); // bias-corrected for heuristic path
int32_t ax_mg = ((int32_t)ax * 1000) / ACCEL_SCALE;
int32_t gz_ddps = ((int32_t)gz * 10) / GYRO_SCALE;
__disable_irq();
int32_t front = (int32_t)FrontDist;
__enable_irq();
int32_t front_filt = median3_i32(front, front_m1, front_m2);
int32_t dt_ms = elapsed ? elapsed : 20;
int32_t v_meas = throttle_to_v_mmps(prev_throttle_avg);
v_hat = v_hat + (((v_meas - v_hat) * V_ALPHA_Q8) >> 8);
int32_t front_pred = front_prev_filt - (v_hat * dt_ms) / 1000;
int32_t r = front_filt - front_pred;
r_acc = ((r_acc * R_ACC_ALPHA_Q8) >> 8) + r;
if (r_acc > R_CAP) r_acc = R_CAP;
if (r_acc < -R_CAP) r_acc = -R_CAP;
int32_t r_thresh = R_THRESH_BASE + ((R_YAW_GAIN_Q8 * abs(gz_ddps)) >> 8);
ot_state = overtake_step(front_filt, r_acc, r_thresh, d2, ld2, gz_ddps, OS_MsTime());
int32_t ramp = (abs(gz_ddps) > GZ_RAMP_GATE) ? OFFSET_RATE/2 : OFFSET_RATE;
dist_ref_cur = ramp_toward(dist_ref_cur, dist_ref_target, ramp);
front_m2 = front_m1; front_m1 = front_filt; front_prev_filt = front_filt;
int32_t angle = arctan(((int32_t)(d_ir*1414) - (int32_t)(d2*1000)) /(int32_t)(224+d2)) - angle_ref;
int32_t L_angle = arctan(((int32_t)(ld_ir*1414) - (int32_t)(ld2*1000))/(int32_t)(224+ld2));
int32_t realDist = (d_ir * cosine(angle)) / 1000;
int32_t L_realDist = (ld_ir * cosine(L_angle)) / 1000;
int32_t e_d = realDist - L_realDist - dist_ref_cur;
int32_t intend_angle = ((kp_d * e_d) / 10) + ((kd_d * (e_d - prevError)) / 10);
prevError = e_d;
int32_t e_a = intend_angle - angle;
int32_t steeringAngle = ((e_a * kp_a) / 10) + (((e_a - prevE_A) * kd_a) / 10);
prevE_A = e_a;
uint16_t throttle = 9990; // max throttle
// Follow the gap kicks in when turn comes up
if(ot_state != S_COMMIT && ot_state != S_PASS && front < 800){
if (front < 600) throttle -= 1000;
if (front < 400) throttle -= 1000;
if (front < 200) throttle -= 1000;
int32_t urgency = (800-front) >> 2; // steer even harder for race day track
steeringAngle = (ld2 > d2) ? -urgency : urgency; // turn left if more room on left
}
// Clamp steering angle
if (steeringAngle < -35) steeringAngle = -35;
else if (steeringAngle > 35) steeringAngle = 35;
if (front < 120){ // too close, tell motors we crashed
// ST7735_Message(1, 1, "AHHH", 0);
CAN_TellCrashed(steeringAngle);
continue;
}
// Differential steering and IMU-based Traction Control / Braking
int32_t t_l = throttle;
int32_t t_r = throttle;
// If we're pulling too many lateral G's, we need to slow down
if (ax_mg > 500 || ax_mg < -500) {
t_l -= 2500;
t_r -= 2500;
}
// If we're supposed to be turning but we're not, differential steering to force us to rotate
if (steeringAngle > 15 && gz_ddps > -500) {
t_l -= 3500;
}
else if (steeringAngle < -15 && gz_ddps < 500) {
t_r -= 3500;
} else {
// Fallback to basic differential steering (older algorithm before IMU)
if (steeringAngle <= -15) {
t_r -= 2000;
}
else if (steeringAngle >= 15) {
t_l -= 2000;
}
}
// Prevent underflow
if (t_l < 0) t_l = 0;
if (t_r < 0) t_r = 0;
uint16_t throttle_l = (uint16_t)t_l;
uint16_t throttle_r = (uint16_t)t_r;
// Normalize inputs to model
// Want to place inputs in range [0, 65536], or [0,1] in fixed point
// Model_Inputs[ir_right] = Model_Normalize(d_ir, CAP_IR);
// Model_Inputs[ir_left] = Model_Normalize(ld_ir, CAP_IR);
// Model_Inputs[tf_left] = Model_Normalize(ld2, CAP_TFLUNA);
// Model_Inputs[tf_middle] = Model_Normalize(front, CAP_TFLUNA);
// Model_Inputs[tf_right] = Model_Normalize(d2, CAP_TFLUNA);
// // Clamp angles
// if (L_angle < -CAP_ANGLE) L_angle = -CAP_ANGLE;
// if (L_angle > CAP_ANGLE) L_angle = CAP_ANGLE;
// if (angle < -CAP_ANGLE) angle = -CAP_ANGLE;
// if (angle > CAP_ANGLE) angle = CAP_ANGLE;
// // Normalize angles
// Model_Inputs[angle_left] = (L_angle + CAP_ANGLE) << (16 - CAP_ANGLE_POW - 1);
// Model_Inputs[angle_right] = (angle + CAP_ANGLE) << (16 - CAP_ANGLE_POW - 1);
// Model_Inputs[yaw_rate] = Model_NormalizeSigned((int32_t)(-gz_raw), CAP_YAW);
// Model_Inputs[accel_lat] = Model_NormalizeSigned((int32_t)ax, CAP_ACCEL);
// Model_Inputs[accel_long] = Model_NormalizeSigned((int32_t)ay, CAP_ACCEL);
// // Call model inference
// Model_Inference();
// // Apply model output to PD controller
// if (ot_state == S_FOLLOW) {
// Model_ApplyResidual(&throttle_l, &throttle_r, &steeringAngle);
// }
// // Wait till after residuals are applied, so input to next is an accurate reflection
// Model_Inputs[throttle_left_prev] = Model_Normalize(throttle_l, CAP_THROTTLE);
// Model_Inputs[throttle_right_prev] = Modesoftwarel_Normalize(throttle_r, CAP_THROTTLE);
// Model_Inputs[steering_prev] = Model_NormalizeSigned(steeringAngle, CAP_STEERING);
ST7735_Message(1, 2, "throt r", throttle_r);
ST7735_Message(1, 3, "throt L", throttle_l);
CAN_SetMotors(throttle_l, throttle_r, steeringAngle);
prev_throttle_avg = ((int32_t)throttle_l + throttle_r) / 2;
// uint32_t sp;
// __asm volatile ("mov %0, sp" : "=r" (sp));
// Data collection
// int32_t row[NUMCOLS] = {OS_MsTime(), elapsed, d2, ld2, front, (int32_t)sp, ot_state};
// if (FileDumpRow(row)){
// EndFileDump();
// char* name;
// unsigned long size;
// eFile_DirNext(&name, &size);
// ST7735_Message(0, 2, "File dump complete ", 0);
// ST7735_Message(0, 3, "File size: ", size);
// while (1){ // Stop robot when we can no longer log
// CAN_SetMotors(0, 0, 0);
// }
// }
}
EndFileDump();
UART_OutString("done.\n\r>");
FileName[5] = (FileName[5]+1)&0xF7; // 0 to 7
Running = 0; // robot no longer running
OS_Kill();
}
//************S2Push*************
// Called when S2 Button pushed, fall of PB21
// Adds another Robot foreground task
// background threads execute once and return
void S2Push(void){
if(Running==0){
Running = 1; // prevents you from starting two test threads
NumCreated += OS_AddThread(&Robot,128,1); // test eDisk
}
}
//--------------end of Task 2-----------------------------
//******** Display ***************
// foreground thread, accepts data from consumer
// displays results on the LCD
// inputs: none
// outputs: none
void Display(void){
uint32_t distance;
uint32_t myId = OS_Id();
ST7735_Message(0,1,"myId = ",myId); // top half used for Display
ST7735_Message(0,2,"Run length = ",(RUNLENGTH)/FS); // top half used for Display
while(Running) {
TogglePB1(); // toggle PB1
distance = OS_MailBox_Recv();
// you will calibrate this in Lab 6
TogglePB1(); // toggle PB1
ST7735_Message(0,3,"Time(ms) =",OS_MsTime());
ST7735_Message(0,4,"work =",FilterWork);
ST7735_Message(0,5,"d(mm) =",distance);
ST7735_Message(0,6,"d IR Raw=", DistanceRaw);
ST7735_Message(0,7,"d IR =", Distance);
if (Distance2 > 2000) Distance2 = 2000;
ST7735_Message(1,0,"wall_angle =", arctan(((int32_t)(Distance*1414) - (int32_t)(Distance2*1000))/(int32_t)(224+Distance2)));
TogglePB1(); // toggle PB1
}
OS_Kill(); // done
}
//--------------end of Task 3-----------------------------
//------------------Task 4--------------------------------
// foreground thread that runs without waiting or sleeping
// it executes a virus detector
uint32_t Check(uint32_t start, uint32_t end){
uint32_t sum=0;
uint32_t *pt; pt = (uint32_t *)start;
while((uint32_t)pt < end){
sum += *pt++;
}
return sum;
}
//******** Virus Detector ***************
// foreground thread, performs a checksum of all ROM
// never blocks, never sleeps, never dies
// inputs: none
// outputs: none
uint32_t Checksum; // sum of data stored in ROM
uint32_t ChecksumOriginal; // sum of data stored in ROM
uint32_t ChecksumErrors;
void VirusDetector(void){
Checks = ChecksumErrors = 0;
ChecksumOriginal = Check(0,0x20000);
while(1) {
TogglePB20(); // toggle PB20
Checksum = Check(0,0x20000);
Checks++;
if(Checksum != ChecksumOriginal){
ChecksumErrors++;
}
}
}
//--------------end of Task 4-----------------------------
//------------------Task 5--------------------------------
// UART0 background ISR performs serial input/output
// Two software fifos are used to pass I/O data to foreground
// The interpreter runs as a foreground thread
// The UART0 driver should call OS_Wait(&RxDataAvailable) when foreground tries to receive
// The UART0 ISR should call OS_Signal(&RxDataAvailable) when it receives data from Rx
// Similarly, the transmit channel waits on a semaphore in the foreground
// and the UART0 ISR signals this semaphore (TxRoomLeft) when getting data from fifo
//******** Interpreter ***************
// Modify your intepreter from Lab 1, adding commands to help debug
// Interpreter is a foreground thread, accepts input from serial port, outputs to serial port
// inputs: none
// outputs: none
void Interpreter(void); // just a prototype, link to your interpreter
// add the following commands, leave other commands, if they make sense
// 1) print performance measures
// time-jitter, number of data points lost, number of calculations performed
// i.e., NumCreated, MaxJitter, DataLost, FilterWork, Checks
// 2) print debugging parameters
// i.e., Checks, ChecksumErrors
// Call these from your interpreter
void Lab4(void){int i;
UART_OutString("\r\nLab 4 performance data");
UART_OutString("\r\nFilterWork = "); UART_OutUDec(FilterWork);
UART_OutString("\r\nNumCreated = "); UART_OutUDec(NumCreated);
UART_OutString("\r\nChecksWork = "); UART_OutUDec(ChecksWork);
UART_OutString("\r\nDataLost = "); UART_OutUDec(DataLost);
UART_OutString("\r\nReal-time sampling jitter (cyc)");
UART_OutString("\r\nTime, Frequencies");
for(i=0; i<JitterSize3; i++){
if(JitterHistogram3[i]){ // skip blanks
UART_OutString("\r\n ");
UART_OutUDec5(i);
UART_OutUDec5(JitterHistogram3[i]);
}
}
UART_OutString("\r\nMaxJitter3(cyc) = "); UART_OutUDec(MaxJitter3);
}
void DFT(void){ int i; int32_t real,imag,mag;
UART_OutString("\r\nLab 2/3 DFT data");
UART_OutString("\r\nInput, Output Real, Output Imaginary, Magnitude");
for(i=0; i<8; i++){
real = ReX[i];
imag = ImX[i];
mag = sqrt2(real*real+imag*imag);
UART_OutString("\r\n"); UART_OutUDec(x[i]); UART_OutChar(' '); UART_OutSDec(real); UART_OutChar(' '); UART_OutSDec(imag);
UART_OutChar(' '); UART_OutSDec(mag);
}
}
//--------------end of Task 5-----------------------------
// IMU Task
void IMU_Task(void){
while (1){
IMU_Read(); // Update globals
// Debugging
// ST7735_Message(1, 1, "AccelX: ", IMU_AccelX);
// ST7735_Message(1, 2, "AccelY: ", IMU_AccelY);
// ST7735_Message(1, 3, "GyroZ: ", IMU_GyroZ);
OS_Sleep(20); // ~50 Hz
}
}
//*******************final user main DEMONTRATE THIS TO TA**********
int realmain(void){ // realmain
OS_Init(); // initialize, disable interrupts
Logic_Init();
DataLost = 0; // lost data between producer and consumer
FilterWork = 0;
Jitter3_Init();
// initialize communication channels
OS_MailBox_Init();
OS_Fifo_Init(256); // ***note*** 4 is not big enough*****
// hardware init
ADC_InitDual(ADC0, 1, 7, ADCVREF_VDDA); // ch1=right IR (PA26), ch7=left IR (PA22)
OS_InitSemaphore(&LCDFree, 1);
OS_InitSemaphore(&TFLuna3Ready, 0); // Robot blocks until ISR produces first sample
OS_InitSemaphore(&TFLuna2Ready, 0);
// CAN init for sending motor commands to motor board
CAN_Init();
CAN_EnableInterrupts(1);
Clock_Delay1ms(20); // Delay for IMU to work
if (IMU_Init() != 0){
ST7735_Message(1, 7, "IMU error", 0);
}
//Initialize LCD
ST7735_InitR(INITR_REDTAB); // Motor board uses SSD1306, not ST7735
// attach background tasks
OS_AddS2Task(&S2Push,1); // fall of PB21
OS_AddPA28Task(&PA28Push,1); // fall of PA28
OS_AddPeriodicThread(&DAS,PERIOD/80000,0); // 1 kHz real time sampling of ADC0_3
OS_AddPeriodicThread(&disk_timerproc,1,0); // time out routines for disk
// create initial foreground threads
NumCreated = 0;
NumCreated += OS_AddThread(&Interpreter,128,3);
NumCreated += OS_AddThread(&Robot,128,1); // CAN motor commands (waits for S2)
NumCreated += OS_AddThread(&IMU_Task, 128, 1); // Gets IMU data
NumCreated += OS_AddThread(&VirusDetector,128,7);
LPF_Init7(500,7);
TFLuna1_Init(&Producer3);
TFLuna1_Format_Standard_mm();
TFLuna1_Frame_Rate();
TFLuna1_SaveSettings();
TFLuna1_System_Reset();
TFLuna2_Init(&Producer2);
TFLuna2_Format_Standard_mm();
TFLuna2_Frame_Rate();
TFLuna2_SaveSettings();
TFLuna2_System_Reset();
TFLuna3_Init(&Producer);
TFLuna3_Format_Standard_mm(); // format in mm
TFLuna3_Frame_Rate(); // 100 samples/sec
TFLuna3_SaveSettings(); // save format and rate
TFLuna3_System_Reset(); // start measurements
if(eFile_Init()) diskError("eFile_Init",0);
if(eFile_Format()) diskError("eFile_Format",0);
if(eFile_Mount()) diskError("eFile_Mount",0);
OS_Launch(TIME_2MS); // doesn't return, interrupts enabled in here
return 0; // this never executes
}
//+++++++++++++++++++++++++DEBUGGING CODE++++++++++++++++++++++++
// ONCE YOUR RTOS WORKS YOU CAN COMMENT OUT THE REMAINING CODE
//
uint32_t M=1;
uint32_t Random32(void){
M = 1664525*M+1013904223;
return M;
}
// 0 to 31
uint32_t Random5(void){
return (Random32()>>27);
}
// 0 to 127
uint32_t Random7(void){
return (Random32()>>25);
}
// 0 to 255
uint8_t Random8(void){
return (Random32()>>24);
}
//*****************Test project 1*************************
// This test should run. If ST7735R works, but this fails:
// - there may be bad solder joints on Sensor board
// - the MSPM0 might have bad pins (PB0 or PB8)
// - SDC not seated correctly or damaged
// Write and read test of random access disk blocks
// Warning: this overwrites whatever is on the disk
unsigned char buffer[512]; // don't put on stack
#define MAXBLOCKS 100
void TestDisk(void){ DSTATUS result; uint32_t block; int i; uint8_t n;
// simple test of eDisk
ST7735_DrawString(0, 1, "eDisk test ", ST7735_WHITE);
UART_OutString("\n\rECE445M, Lab 4 eDisk test\n\rTestmain1\n\r");
result = eDisk_Init(0); // initialize disk
if(result) diskError("eDisk_Init",result);
UART_OutString("Writing blocks\n\r");
M = 1; // seed
for(block = 0; block < MAXBLOCKS; block++){
for(i=0;i<512;i++){
buffer[i] = Random8();
}
SetPA8(); // PA8 high for 100 block writes
if(eDisk_WriteBlock(buffer,block))diskError("eDisk_WriteBlock",block); // save to disk
ClrPA8();
}
UART_OutString("Reading blocks\n\r");
M = 1; // reseed, start over to get the same sequence
for(block = 0; block < MAXBLOCKS; block++){
SetPB23(); // PB23 high for one block read
if(eDisk_ReadBlock(buffer,block))diskError("eDisk_ReadBlock",block); // read from disk
ClrPB23();
for(i=0;i<512;i++){
n = Random8(); // same pseudo random sequence
if(buffer[i] != (0xFF&n)){
UART_OutString("Read data not correct, block="); UART_OutUDec(block);
UART_OutString(", i="); UART_OutUDec(i);
UART_OutString(", expected "); UART_OutUDec(0xFF&n);
UART_OutString(", read "); UART_OutUDec(buffer[i]);
UART_OutString("\n\r");
OS_Kill();
}
}
}
UART_OutString("Successful test of 100 blocks\n\r");
ST7735_DrawString(0, 1, "eDisk successful", ST7735_YELLOW);
Running = 0; // allow launch again
OS_Kill();
}
void StartTestDisk(void){
if(Running==0){
Running = 1; // prevents you from starting two test threads
NumCreated += OS_AddThread(&TestDisk,128,1); // test eDisk
}
}
int Testmain1(void){ // Testmain1
OS_Init(); // initialize, disable interrupts
// OS_AddDevices(1,1,0); // attach printf to UART0, allow ST7735, not eFile
Logic_Init();
Running = 1;
// attach background tasks
OS_AddPeriodicThread(&disk_timerproc,1,0); // time out routines for disk
OS_AddS2Task(&StartTestDisk,1);
OS_AddPA28Task(&StartTestDisk,1);
// create initial foreground threads
NumCreated = 0 ;
NumCreated += OS_AddThread(&TestDisk,128,1);
NumCreated += OS_AddThread(&VirusDetector,128,3);
OS_Launch(TIME_2MS); // doesn't return, interrupts enabled in here
return 0; // this never executes
}
//*******************Measurement of context switch time**********
// Run this to measure the time it takes to perform a task switch
// UART0 not needed
// SYSTICK interrupts, period established by OS_Launch
// first timer not needed
// second timer not needed
// S1 not needed,
// S2 not needed
// logic analyzer on PB22 for systick interrupt (in your OS)
// on PA8 to measure context switch time
void ThreadCS(void){ // only thread running
while(1){
TogglePA8(); // toggle PA8
}
}
int TestmainCS(void){ // TestmainCS
Logic_Init();
OS_Init(); // initialize, disable interrupts
NumCreated = 0 ;
NumCreated += OS_AddThread(&ThreadCS,128,0);
OS_Launch(TIME_1MS/10); // 100us, doesn't return, interrupts enabled in here
return 0; // this never executes
}
//*****************Test project 2*************************
// Filesystem test.
// Warning: this reformats the disk, all existing data will be lost
void PrintDirectory(void){ char *name; unsigned long size;
unsigned int num;
unsigned long total;
num = 0;
total = 0;
UART_OutString("\n\r");
if(eFile_DOpen("")) diskError("eFile_DOpen",0);
while(!eFile_DirNext(&name, &size)){
UART_OutString("Filename = "); UART_OutString(name); UART_OutString(" ");
UART_OutString("Size (bytes)= "); UART_OutUDec(size); UART_OutString("\n\r");
total = total+size;
num++;
}
UART_OutString("Number of Files = "); UART_OutUDec(num); UART_OutString("\n\r");
UART_OutString("Number of Bytes = "); UART_OutUDec(total); UART_OutString("\n\r");
if(eFile_DClose()) diskError("eFile_DClose",0);
}
void TestFile(void){ int i; char data; int status;
UART_OutString("\n\rECE445M Lab 4 eFile test 2\n\r");
ST7735_DrawString(0, 1, "eFile test 2 ", ST7735_WHITE);
// simple test of eFile
if(eFile_Init()) diskError("eFile_Init",0);
if(eFile_Format()) diskError("eFile_Format",0);
if(eFile_Mount()) diskError("eFile_Mount",0);
PrintDirectory();
if(eFile_Create("file1")) diskError("eFile_Create",0);
if(eFile_WOpen("file1")) diskError("eFile_WOpen",0);
for(i=5; i<=15; i++){
eFile_WriteString("Testmain2\tabcdefghijklmnopqrstuvwxyz\t");
eFile_WriteUFix2(OS_MsTime()/10); eFile_Write('\t');
eFile_WriteUDec(i); eFile_Write('\t');
eFile_WriteSFix2(i-10); eFile_Write('\t');
eFile_WriteSDec(i-10); eFile_WriteString("\n\r");
OS_Sleep(10);
}
if(eFile_WClose()) diskError("eFile_WClose",0);
PrintDirectory();
if(eFile_ROpen("file1")) diskError("eFile_ROpen",0);
do{
status = eFile_ReadNext(&data);
if(status == 0) UART_OutChar(data);
}while(status==0);
if(eFile_RClose()) diskError("eFile_RClose",0);
if(eFile_Delete("file1")) diskError("eFile_Delete",0);
PrintDirectory();
if(eFile_Unmount()) diskError("eFile_Unmount",0);
UART_OutString("Successful test\n\r");
ST7735_DrawString(0, 1, "eFile successful", ST7735_YELLOW);
Running=0; // launch again
OS_Kill();
}
void StartFileTest(void){
if(Running==0){
Running = 1; // prevents you from starting two test threads
NumCreated += OS_AddThread(&TestFile,128,1); // test eFile
}
}
int Testmain2(void){ // Testmain2
OS_Init(); // initialize, disable interrupts
Logic_Init();
Running = 1;
// attach background tasks
OS_AddPeriodicThread(&disk_timerproc,1,0); // time out routines for disk
OS_AddS2Task(&StartFileTest,1);
OS_AddPA28Task(&StartFileTest,1);
// create initial foreground threads
NumCreated = 0 ;
NumCreated += OS_AddThread(&TestFile,128,1);
NumCreated += OS_AddThread(&VirusDetector,128,3);
OS_Launch(TIME_2MS); // doesn't return, interrupts enabled in here
return 0; // this never executes
}
void PrintFile3(char *pt){int status; char data;
OS_bWait(&LCDFree);
eFile_ROpen(pt);
do{
status = eFile_ReadNext(&data);
if(status == 0) UART_OutChar(data);
}while(status==0);
eFile_RClose();
OS_bSignal(&LCDFree);
}
void Dump3(uint32_t run,int32_t data){
SetPA8();
OS_bWait(&LCDFree);
eFile_WriteString("Testmain3\tabcdefghijklmnopqrstuvwxyz\t");
eFile_WriteUFix2(OS_MsTime()/10); eFile_Write('\t');
eFile_WriteUDec(run); eFile_Write('\t');
eFile_WriteSFix2(data); eFile_Write('\t');
eFile_WriteSDec(data); eFile_WriteString("\n\r");
OS_bSignal(&LCDFree);
ClrPA8();
}
//*****************Test project 3*************************
// Filesystem stream test.
// Warning: this reformats the disk, all existing data will be lost
uint32_t Run3=0;
void TestFile3(void){ int i; char data;
UART_OutString("\n\rECE445M Lab 4 eFile test 3\n\r");
ST7735_Message(0,1,"eFile test 3", Run3);
// test of eFile
PrintDirectory();
OS_bWait(&LCDFree);
eFile_Create(FileName);
eFile_WOpen(FileName);
OS_bSignal(&LCDFree);
for(i=-5; i<=5; i++){
Dump3(Run3,i);
Run3++;
OS_Sleep(10);
}
OS_bWait(&LCDFree);
eFile_WClose();
OS_bSignal(&LCDFree);
PrintDirectory();
PrintFile3(FileName);
UART_OutString("Successful test 3, Run3="); UART_OutUDec(Run3);
UART_OutString("\n\r");
ST7735_Message(0,2,"Run3 =",Run3);
Running = 0; // allowed to launch again
FileName[5] = (FileName[5]+1)&0xF7; // 0 to 7
OS_Kill();
}
void Chaos3(void){
ST7735_Message(1,0,"Chaos",3);
while(1){
for(int l=1; l<5; l++){
ST7735_Message(1,l,"n =",Random8());
}
OS_Sleep(100);
}
}
void StartFileTest3(void){
if(Running==0){
Running = 1; // prevents you from starting two test threads
NumCreated += OS_AddThread(&TestFile3,128,1); // test eFile
}
}
int Testmain3(void){ // Testmain3
OS_Init(); // initialize, disable interrupts
Logic_Init();
Running = 1;
OS_InitSemaphore(&LCDFree, 1);
// attach background tasks
OS_AddPeriodicThread(&disk_timerproc,1,0); // time out routines for disk
OS_AddS2Task(&StartFileTest3,1);
OS_AddPA28Task(&StartFileTest3,1);
// create initial foreground threads
NumCreated = 0 ;
NumCreated += OS_AddThread(&TestFile3,128,1);
NumCreated += OS_AddThread(&Chaos3,128,1);
NumCreated += OS_AddThread(&VirusDetector,128,3);
if(eFile_Init()) diskError("eFile_Init",0);
if(eFile_Format()) diskError("eFile_Format",0);
if(eFile_Mount()) diskError("eFile_Mount",0);
OS_Launch(TIME_2MS); // doesn't return, interrupts enabled in here
return 0; // this never executes
}
//*****************Dump robot0 over UART*************************
// Press S2 to stream the contents of "robot0" out UART0.
// Capture on host with log_serial.sh (at repo root).
// Does NOT format the disk — preserves logged data.
void DumpRobotFile(void){
PrintFile3("robot0");
Running = 0; // allow re-trigger via S2
OS_Kill();
}
void StartDumpRobotFile(void){
if(Running == 0){
Running = 1;
NumCreated += OS_AddThread(&DumpRobotFile, 128, 1);
}
}
int DumpRobotFileMain(void){
OS_Init();
Logic_Init();
Running = 0;
OS_InitSemaphore(&LCDFree, 1);
OS_AddPeriodicThread(&disk_timerproc, 1, 0);
OS_AddS2Task(&StartDumpRobotFile, 1);
OS_AddPA28Task(&StartDumpRobotFile, 1);
NumCreated = 0;
NumCreated += OS_AddThread(&VirusDetector, 128, 7);
NumCreated += OS_AddThread(&Interpreter, 128, 1);
if(eFile_Init()) diskError("eFile_Init", 0);
if(eFile_Mount()) diskError("eFile_Mount", 0);
UART_OutString("\n\rReady. Press S2 (PB21) to dump robot0.\n\r");
OS_Launch(TIME_2MS);
return 0;
}
//*****************Test project IMU*************************
// Read IMU, scale to physical units, display on LCD.
// Accel in milli-g (1000 mg = 1 g).
// Gyro in deci-dps (10 ddps = 1 deg/s).
// Temp in centi-C (100 cC = 1 C).
void TestIMU(void){
while(1){
IMU_Read();
int32_t ax_mg = ((int32_t)IMU_AccelX * 1000) / 16384;
int32_t ay_mg = ((int32_t)IMU_AccelY * 1000) / 16384;
int32_t az_mg = ((int32_t)IMU_AccelZ * 1000) / 16384;
int32_t gx_ddps = ((int32_t)IMU_GyroX * 10) / 131;
int32_t gy_ddps = ((int32_t)IMU_GyroY * 10) / 131;
int32_t gz_ddps = ((int32_t)IMU_GyroZ * 10) / 131;
int32_t t_cC = ((int32_t)IMU_Temp * 100) / 340 + 3653;
ST7735_Message(0, 0, "ax (mg) =", ax_mg);
ST7735_Message(0, 1, "ay (mg) =", ay_mg);
ST7735_Message(0, 2, "az (mg) =", az_mg);
ST7735_Message(0, 3, "gx(ddps) =", gx_ddps);
ST7735_Message(0, 4, "gy(ddps) =", gy_ddps);
ST7735_Message(0, 5, "gz(ddps) =", gz_ddps);
ST7735_Message(0, 6, "T (cC) =", t_cC);
OS_Sleep(100); // 10 Hz display update
}
}
int TestmainIMU(void){
OS_Init();
Logic_Init();
ST7735_InitR(INITR_REDTAB);
int err = IMU_Init();
if(err != 0){
ST7735_Message(0, 0, "IMU_Init err =", err);
while(1){} // halt
}
NumCreated = 0;
NumCreated += OS_AddThread(&TestIMU, 128, 1);
NumCreated += OS_AddThread(&VirusDetector, 128, 2);
OS_Launch(TIME_2MS);
return 0;
}
//*******************Trampoline for selecting which main to execute**********
int main(void) { // main
__disable_irq();
Clock_Init80MHz(0); // no clock out to pin
LaunchPad_Init(); // LaunchPad_Init must be called once and before other I/O initializations
realmain();
}
Reference in New Issue Copy Permalink