Appendix: Code
/*
* File: tank.c
* Author: lmm343, mah426, chk57
*
* Created on November 4, 2021, 2:57 PM
*/
#include
#include "config_1_3_3.h"
// threading library
#include "pt_cornell_1_3_3.h"
// For below libraries comment out when transferring to other board
////////////////////////////////////
// graphics libraries
// SPI channel 1 connections to TFT
// #include "tft_master.h"
// #include "tft_gfx.h"
// need for rand function
#include
// need for sine function
#include
// The fixed point types
#include
////////////////////////////////////
// Speed of sound in m/s
#define SPEED_OF_SOUND 340.0
/* US Use note
* travel distance of ultrasonic wave = (speed of sound) x (echo pulse duration)
* distance from sensor to object = (travel_distance)/2
*
* Can trigger a timer using input compare module on the detect of a rising edge
* stop timer on falling edge --> gets duration of on timer for pulse
*/
#define PBCLK 40000000 // peripheral bus clock
#define del_us PBCLK/2000000
int generate_period = 40000;
volatile int pwm_on_time1 = 35000;
volatile int pwm_on_time2 = 35000;
volatile int move = 0;
volatile int back = 0;
volatile float duration;
volatile float dist;
float last_dist = 0;
//The measured period of the wave
volatile unsigned int t=0, t_prev=0, pulse_width=0;
volatile int rising = 1;
// Software delay of i microseconds
void delay_micro(unsigned long i){
unsigned int j;
j = del_us * i;
WriteCoreTimer(0);
while (ReadCoreTimer() < j);
}
//==========================Timer for 1kHz sample rate==========================
void __ISR(_TIMER_2_VECTOR, ipl2) Timer2Handler(void)
{
mT2ClearIntFlag();
if ( dist > 15 && move == 1 ) { //dist > 5 &&
SetDCOC3PWM(pwm_on_time1);
SetDCOC4PWM(pwm_on_time2);
} else if (move == 1 && back == 1) {
SetDCOC3PWM(pwm_on_time1);
SetDCOC4PWM(pwm_on_time2);
} else {
SetDCOC3PWM(0);
SetDCOC4PWM(0);
}
}
volatile int debounce = 0;
// == Capture 1 ISR ====================================================
// check every cpature for consistency
void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl3) C1Handler(void)
{
// read the capture register
t = mIC1ReadCapture();
if (rising == 0) {
// compute the repeating interval
pulse_width = t;
// duration = (float)pulse_width*(1.0/40000000.0);
// dist = ((SPEED_OF_SOUND*duration))*50;
rising = 1;
}else {
WriteTimer3(0x0);
t_prev = 0;
rising = 0;
}
// clear the timer interrupt flag
mIC1ClearIntFlag();
}
// === print a line on TFT =====================================================
// Utilities to print a line on the TFT
// Predefined colors definitions (from tft_master.h)
//#define ILI9340_BLACK 0x0000
//#define ILI9340_BLUE 0x001F
//#define ILI9340_RED 0xF800
//#define ILI9340_GREEN 0x07E0
//#define ILI9340_CYAN 0x07FF
//#define ILI9340_MAGENTA 0xF81F
//#define ILI9340_YELLOW 0xFFE0
//#define ILI9340_WHITE 0xFFFF
// string buffer
//char buffer[60];
//void printLine(int line_number, char* print_buffer, short text_color, short back_color){
// // line number 0 to 31
// /// !!! assumes tft_setRotation(0);
// // print_buffer is the string to print
// int v_pos;
// v_pos = line_number * 10 ;
// // erase the pixels
// tft_fillRoundRect(0, v_pos, 239, 8, 1, back_color);// x,y,w,h,radius,color
// tft_setTextColor(text_color);
// tft_setCursor(0, v_pos);
// tft_setTextSize(1);
// tft_writeString(print_buffer);
//}
void move_robot(char* cmd) {
char buffer[30];
if (cmd[2] == '5' && cmd[3] == '1') { // move the robot forward
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "forward");
pwm_on_time1 = 35000;
pwm_on_time2 = 35000;
// if (dist > 10){
mPORTBClearBits(BIT_3);
mPORTBClearBits(BIT_5);
mPORTASetBits(BIT_3);
mPORTBSetBits(BIT_4);
// } else {
// mPORTBClearBits(BIT_3);
// mPORTBClearBits(BIT_5);
// mPORTAClearBits(BIT_3);
// mPORTBClearBits(BIT_4);
// }
move = 1;
back = 0;
} else if (cmd[2] == '6' && cmd[3] == '1') { // move the robot back
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "back");
pwm_on_time1 = 35000;
pwm_on_time2 = 35000;
mPORTBSetBits(BIT_3);
mPORTBSetBits(BIT_5);
mPORTAClearBits(BIT_3);
mPORTBClearBits(BIT_4);
move = 1;
back = 1;
} else if (cmd[2] == '7' && cmd[3] == '1') { // move the robot left
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "left");
pwm_on_time1 = 35000;
pwm_on_time2 = 35000;
mPORTBSetBits(BIT_3);
mPORTBClearBits(BIT_5);
mPORTAClearBits(BIT_3);
mPORTBSetBits(BIT_4);
move = 1;
back = 0;
} else if (cmd[2] == '8' && cmd[3] == '1') { // move the robot right
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "right");
pwm_on_time1 = 35000;
pwm_on_time2 = 35000;
mPORTBClearBits(BIT_3);
mPORTBSetBits(BIT_5);
mPORTASetBits(BIT_3);
mPORTBClearBits(BIT_4);
move = 1;
back = 0;
} else if (cmd[2] == '1' && cmd[3] == '1') {
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "shoot");
mPORTBSetBits(BIT_14);
move = 0;
back = 0;
} else if (cmd[3] == '0') { // stop the robot if button 1 was pressed
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer, "stop");
move = 0;
back = 0;
mPORTBClearBits(BIT_14);
}
}
//=== Serial terminal thread =================================================
// simple command interpreter and serial display
// BUT NOTE that there are TWO completely different modes of getting strings
// from the UART. ONe assumes a human is typing, the other assumes a machine
//
// Also -- AUX uart loopback transmit thread using the 's' command
// semaphores to sync threads
int ready_to_send=1, ready_to_receive=0 ;
// NOTE that command 's' below sends/receives data via the
// UART1 AUX machine communcation channel.
// mode==1 is command mode; 2 is data mode
static int mode = 1;
static PT_THREAD (protothread_serial(struct pt *pt))
{
PT_BEGIN(pt);
static char cmd[30], t0;
static char prev_cmd[30];
static float value;
static int i;
static int go = 0;
static int v1, v2;
static char buffer[30];
// string recognized by module to toggle data<->command
static char cmd_data_toggle[] = "+++\r";
// termination for machine buffer
PT_terminate_char = '#' ; // see http://www.asciitable.com/
PT_terminate_count = 0 ;
// time in milliseconds!
PT_terminate_time = 0 ;
// semaphores for sync two thread
while(1) {
// get config or move cmd from bt module
PT_SPAWN(pt, &pt_DMA_input_aux, PT_GetMachineBuffer_aux(&pt_DMA_input_aux) );
sscanf(PT_term_buffer_aux, "%s %f", cmd, &value);
// ==============================================
// At this point, we have a valid string buffer
// Now parse it:
// === command parser ================================
switch(cmd[0]) {
case 'm':
// toggle the bluetooth module mode between 'command' and 'data'
// mode==1 is command mode
if (mode == 1) mode=2; // go to data mode
else mode = 1; // goto command mode
// now send the mode change to the module
strcpy(PT_send_buffer_aux, cmd_data_toggle);
PT_YIELD_UNTIL(pt, ready_to_send==1);
ready_to_receive = 1;
ready_to_send = 0 ;
PT_YIELD(pt);
PT_SPAWN(pt, &pt_DMA_output_aux, PT_DMA_PutSerialBuffer_aux(&pt_DMA_output_aux) );
break;
case 's':
// read a string into the aux send buffer
// IN COMMAND MODE:
// -- returns bluetooth module response
// IN DATA MODE:
// -- returns string from remote bluetooth device
// -- hangs until it gets a response with '\r' terminator
//
// get the string (command or payload data) to send to the module
sscanf(PT_term_buffer_aux, "%s %s", cmd, PT_send_buffer_aux);
strcat(PT_send_buffer_aux, "\r");
//
//send a string to bluetooth module thru UART1 (AUX))
// wait for the read to be done
PT_YIELD_UNTIL(pt, ready_to_send==1);
// start the read, THEN start the write
// signal the receive thread that data is coming
ready_to_receive = 1;
// clear the ready flag until read thread is done
ready_to_send = 0 ;
// wait a little so that receive thread is ready
// and the DMA channel is ready
PT_YIELD(pt);
// send using AUX USART
// NOTE: set as ending character (of a string) in PT_DMA_PutSerialBuffer_aux
// REQUIRED by ADAFRUIT SERIAL FRIEND
// setting the end character to NULL transmits the NULL,
// which confuses the module
PT_SPAWN(pt, &pt_DMA_output_aux, PT_DMA_PutSerialBuffer_aux(&pt_DMA_output_aux) );
// wait for the AUX receive to actually happen
// (see receive thread))
// (including time outs)
PT_YIELD_UNTIL(pt, ready_to_send==1);
break;
case '!': // Signals that a button from the control pad was pressed
go = 1;
move_robot(cmd);
// case 'g':
// go = 0;
// if (go == 0) {
// sprintf(buffer, "%f", dist);
// sprintf(buffer, "%f , %d", dist, pulse_width);
//
// strcpy(PT_send_buffer_aux, buffer);
//
// strcat(PT_send_buffer_aux, "\r");
// //
// //send a string to bluetooth module thru UART1 (AUX))
// // wait for the read to be done
// PT_YIELD_UNTIL(pt, ready_to_send==1);
// // start the read, THEN start the write
// // signal the receive thread that data is coming
// ready_to_receive = 1;
// // clear the ready flag until read thread is done
// ready_to_send = 0 ;
// // wait a little so that receive thread is ready
// // and the DMA channel is ready
// PT_YIELD(pt);
//
// // send using AUX USART
// // NOTE: set as ending character (of a string) in PT_DMA_PutSerialBuffer_aux
// // REQUIRED by ADAFRUIT SERIAL FRIEND
// // setting the end character to NULL transmits the NULL,
// // which confuses the module
// PT_SPAWN(pt, &pt_DMA_output_aux, PT_DMA_PutSerialBuffer_aux(&pt_DMA_output_aux) );
//
// // wait for the AUX receive to actually happen
// // (see receive thread))
// // (including time outs)
// PT_YIELD_UNTIL(pt, ready_to_send==1);
//
// }
//
// case 'f':
// go == 1;
//
//
} // end switch statement
// never exit while
} // END WHILE(1)
PT_END(pt);
} // thread serial
// ========================================================
// AUX uart receive thread
static PT_THREAD (protothread_serial_aux(struct pt *pt))
{
PT_BEGIN(pt);
// termination for machine buffer on AUX channel
// terminate on '#' OR 6 caracters OR 1000 mSec
// whichever comes first
PT_terminate_char_aux = 0 ; // see http://www.asciitable.com/
PT_terminate_count_aux = max_chars_aux ;
// time in milliseconds!
PT_terminate_time_aux = 500 ;
while(1) {
if (mode==1) { // command mode. No required, so can receive multiple lines
PT_terminate_char_aux = 0 ; // allow multiple line receive
PT_terminate_count_aux = max_chars_aux ;
// time in milliseconds!
PT_terminate_time_aux = 500 ;
// wait for data transmission start
}
else { // data mode terminated by , no time limit
PT_terminate_char_aux = '\r' ; // see http://www.asciitable.com/
PT_terminate_count_aux = max_chars_aux ;
// time in milliseconds!
PT_terminate_time_aux = 0 ; // no time out for radio respose
}
PT_YIELD_UNTIL(pt, ready_to_receive==1);
ready_to_receive = 0 ;
// get sent data on AUX channel (UART1 loopback)
PT_SPAWN(pt, &pt_DMA_input_aux, PT_GetMachineBuffer_aux(&pt_DMA_input_aux) );
// reset semaphore to indicate data received
// (or timed out)
ready_to_send = 1;
// NEVER exit while
} // END WHILE(1)
PT_END(pt);
} // aux uart thread
/*
* IN1 --> RPB3
* IN2 --> RPA3
* IN3 --> RPB4
* IN4 --> RPB6
*/
int num = 0;
static PT_THREAD (protothread_us(struct pt *pt)) {
PT_BEGIN(pt);
static char buffer[128];
static float new_dist;
while(1) {
PT_YIELD_TIME_msec(70);
mPORTASetBits(BIT_0);
delay_micro(10);
mPORTAClearBits(BIT_0);
duration = (float)pulse_width*(1.0/5000000.0);
dist = ((SPEED_OF_SOUND*duration))*50;
// tft_fillRoundRect(0,0, 120, 50, 1, ILI9340_BLACK);// x,y,w,h,radius,color
// tft_setTextColor(ILI9340_WHITE);
// tft_setCursor(5, 20);
// tft_setTextSize(1);
// tft_writeString(buffer);
// sprintf(buffer,"dist=%f ", dist);
}
PT_END(pt);
}
/*
*
*/
void main(void) {
ANSELA = 0; ANSELB = 0;
PT_setup();
// timer interrupt //////////////////////////
// Set up timer3 on, interrupts, internal clock, prescalar 1, toggle rate
// at 40 MHz PB clock +
// 40,000,000/1000 = 40000 : since timer is zero-based, set to 39999
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 39999);
// set up the timer interrupt with a priority of 2
ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
mT2ClearIntFlag(); // and clear the interrupt flag
// timer interrupt //////////////////////////
// Set up timer3 on, interrupts, internal clock, prescalar 1, toggle rate
// at 40 MHz PB clock
// Triggers every 100us
// 5,000,000/1000 = 5000 : since timer is zero-based, set to 3999
OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_8, 0xFFFF);
mT3ClearIntFlag(); // and clear the interrupt flag
// set up compare3 for double compare mode
// first number is the time to clear, second is the time to set the pin
// in this case, the end of the timer period and 50% of the timer period
OpenOC3(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, generate_period-1, generate_period>>1); //
// OC3 is PPS group 4, map to RPB9 (pin 18)
PPSOutput(4, RPB9, OC3);
// set up compare3 for double compare mode
// first number is the time to clear, second is the time to set the pin
// in this case, the end of the timer period and 50% of the timer period
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, generate_period-1, generate_period>>1); //
// OC3 is PPS group 4, map to RPB9 (pin 18)
PPSOutput(3, RPA2, OC4);
// set up input compare module
OpenCapture1( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER3_SRC | IC_ON );
ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_3 | IC_INT_SUB_PRIOR_3 );
INTClearFlag(INT_IC1);
// connect PIN 24 to IC3 capture unit
PPSInput(3, IC1, RPA4);
mPORTBSetPinsDigitalOut(BIT_3 | BIT_5 | BIT_4 | BIT_14); // BIT_14 is for gun
mPORTASetPinsDigitalOut(BIT_3 | BIT_0); // BIT_0 is for the US sensor
mPORTAClearBits(BIT_3 | BIT_0);
mPORTBClearBits(BIT_10 | BIT_14 | BIT_5 | BIT_3 | BIT_4);
// // init the display
// // NOTE that this init assumes SPI channel 1 connections
// tft_init_hw();
// tft_begin();
// tft_fillScreen(ILI9340_BLACK);
// //240x320 vertical display
// tft_setRotation(0); // Use tft_setRotation(1) for 320x240
// === setup system wide interrupts ========
INTEnableSystemMultiVectoredInt();
// === identify the threads to the scheduler =====
// add the thread function pointers to be scheduled
// --- Two parameters: function_name and rate. ---
// rate=0 fastest, rate=1 half, rate=2 quarter, rate=3 eighth, rate=4 sixteenth,
// rate=5 or greater DISABLE thread!
pt_add(protothread_us, 0);
pt_add(protothread_serial, 1);
pt_add(protothread_serial_aux, 0);
// === initalize the scheduler ====================
PT_INIT(&pt_sched);
// >>> CHOOSE the scheduler method: <<<
// (1)
// SCHED_ROUND_ROBIN just cycles thru all defined threads
//pt_sched_method = SCHED_ROUND_ROBIN ;
// NOTE the controller must run in SCHED_ROUND_ROBIN mode
// ALSO note that the scheduler is modified to cpy a char
// from uart1 to uart2 for the controller
pt_sched_method = SCHED_ROUND_ROBIN;
// === scheduler thread =======================
// scheduler never exits
PT_SCHEDULE(protothread_sched(&pt_sched));
// ============================================
}
