adipu/TweinStein
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
30406f4f49aab497aad4bc737915b6668a940337
TweinStein/RTOS_SensorBoard/RTOS_SensorBoard.c
pulipakaa24 30406f4f49 all
2026-06-12 02:55:04 -07:00

1390 lines
48 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/* 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 View Git Blame Copy Permalink