Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
Software apps and online services | ||||||
 |
| |||||
This project uses the TI Launchpad and the custom designed segbot by Professor Dan Block. This project incorporates three main components in order to function. It utilizes the DC motors with rotary encoders, a SG90 servo motor, and an ultrasonic sensor. The ultrasonic sensor is mounted on top of the servo motor. Essentially, the Ultra Sonic Sensor utilizes the WIZNET to read data from the echo on the ultrasonic sensor. This data is converted into centimeters. This distance information then commands the motors to both back up & then rotate opposite directions and once done, continue driving forward.
In order to tell what direction the segbot should turn, I implemented my own algorithm that essentially flags when the ultrasonic sensor comes within 20.0 cm of the ultrasonic sensor, then backs up for 1 second, sends a PWM signal to the servo motor to turn 70 degrees left and right of the segbot and then read the echo distance from the ultrasonic sensor. If the Left Echo reading is greater than the Right Echo reading, the segbot turns right because there is theoretically more room to continue. If the Right Echo reading is greater than the Left Echo reading, the segbot turns left because there is theoretically more room to continue in this direction. In order to turn, the PWM signal is sent to the motors in opposite directions in accordance with the desired direction for turning.
Explanation:
Robot Driving Around:
//#############################################################################
// FILE: labstarter_main.c
//
// TITLE: Lab Starter
//#############################################################################
// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#include "LEDPatterns.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define TIMEBASE 0.005 // 1.0/200
//--------------------------------------------AUDIO code-------------------------------//
//~~~~~~~~~~Take on Me notes~~~~~~~~~~~~~~~~~
#define OFFNOTE 0
#define SONG_LENGTH 48
#define SONG2_LENGTH 95
#define NOTE_FS5 ((uint16_t)(((50000000/2)/2)/739.989))
#define NOTE_D5 ((uint16_t)(((50000000/2)/2)/587.33))
#define NOTE_B4 ((uint16_t)(((50000000/2)/2)/493.88))
#define NOTE_E5 ((uint16_t)(((50000000/2)/2)/659.25))
#define NOTE_GS5 ((uint16_t)(((50000000/2)/2)/830.61))
#define NOTE_A5 ((uint16_t)(((50000000/2)/2)/880.00))
#define NOTE_B5 ((uint16_t)(((50000000/2)/2)/987.77))
uint16_t countToSongLength = 0;
uint16_t start_song = 0;
uint16_t song2array[SONG2_LENGTH] = {
NOTE_FS5,
NOTE_FS5,
NOTE_D5,
NOTE_B4,
OFFNOTE,
NOTE_B4,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
NOTE_GS5,
NOTE_GS5,
NOTE_A5,
NOTE_B5,
NOTE_A5,
NOTE_A5,
NOTE_A5,
NOTE_E5,
OFFNOTE,
NOTE_D5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
NOTE_E5,
NOTE_E5,
NOTE_FS5,
NOTE_E5,
NOTE_FS5,
NOTE_FS5,
NOTE_D5,
NOTE_B4,
OFFNOTE,
NOTE_B4,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
NOTE_GS5,
NOTE_GS5,
NOTE_A5,
NOTE_B5,
NOTE_A5,
NOTE_A5,
NOTE_A5,
NOTE_E5,
OFFNOTE,
NOTE_D5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
NOTE_E5,
NOTE_E5,
NOTE_FS5,
NOTE_E5,
NOTE_FS5,
NOTE_FS5,
NOTE_D5,
NOTE_B4,
OFFNOTE,
NOTE_B4,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
OFFNOTE,
NOTE_E5,
NOTE_GS5,
NOTE_GS5,
NOTE_A5,
NOTE_B5,
NOTE_A5,
NOTE_A5,
NOTE_A5,
NOTE_E5,
OFFNOTE,
NOTE_D5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
OFFNOTE,
NOTE_FS5,
NOTE_E5,
NOTE_E5,
NOTE_FS5,
NOTE_E5};
// VCC - +5V
// GND - GND
// TRIG - GPIO0
// ECHO - GPIO19
int32_t echo_count = 0;
float echo_duration = 0;
float echo_distance = 0;
uint16_t trigger_count = 0;
uint16_t trigger_state = 0;
float echo_average = 40.0;
float echo_distance_1 = 0;
float echo_distance_2 = 0;
float echo_distance_3 = 0;
uint16_t turn_flag = 0;
// This function should be called every 1ms
void hc_sr04_trigger(void) {
if (trigger_count < 2) {
if (trigger_state == 0) {
// last for 2ms
GpioDataRegs.GPACLEAR.bit.GPIO0 = 1;
trigger_state = 1;
}
trigger_count++;
} else if ( (trigger_count >= 2) && (trigger_count < 11) ) {
// last for 10ms
if (trigger_state == 1) {
GpioDataRegs.GPASET.bit.GPIO0 = 1;
trigger_state = 2;
}
trigger_count++;
} else {
if (trigger_state == 2) {
trigger_count = 0;
trigger_state = 0;
}
}
}
// InitECapture - Initialize ECAP1 configurations
void InitECapture() {
EALLOW;
DevCfgRegs.SOFTPRES3.bit.ECAP1 = 1; // eCAP1 is reset
DevCfgRegs.SOFTPRES3.bit.ECAP1 = 0; // eCAP1 is released from reset
EDIS;
ECap1Regs.ECEINT.all = 0; // Disable all eCAP interrupts
ECap1Regs.ECCTL1.bit.CAPLDEN = 0; // Disabled loading of capture results
ECap1Regs.ECCTL2.bit.TSCTRSTOP = 0; // Stop the counter
ECap1Regs.TSCTR = 0; // Clear the counter
ECap1Regs.CTRPHS = 0; // Clear the counter phase register
// ECAP control register 2
ECap1Regs.ECCTL2.all = 0x0096;
// bit 15-11 00000: reserved
// bit 10 0: APWMPOL, don't care
// bit 9 0: CAP/APWM, 0 = capture mode, 1 = APWM mode
// bit 8 0: SWSYNC, 0 = no action (no s/w synch)
// bit 7-6 10: SYNCO_SEL, 10 = disable sync out signal
// bit 5 0: SYNCI_EN, 0 = disable Sync-In
// bit 4 1: TSCTRSTOP, 1 = enable counter
// bit 3 0: RE-ARM, 0 = don't re-arm, 1 = re-arm
// bit 2-1 11: STOP_WRAP, 11 = wrap after 4 captures
// bit 0 0: CONT/ONESHT, 0 = continuous mode
// ECAP control register 1
ECap1Regs.ECCTL1.all = 0xC144;
// bit 15-14 11: FREE/SOFT, 11 = ignore emulation suspend
// bit 13-9 00000: PRESCALE, 00000 = divide by 1
// bit 8 1: CAPLDEN, 1 = enable capture results load
// bit 7 0: CTRRST4, 0 = do not reset counter on CAP4 event
// bit 6 1: CAP4POL, 0 = rising edge, 1 = falling edge
// bit 5 0: CTRRST3, 0 = do not reset counter on CAP3 event
// bit 4 0: CAP3POL, 0 = rising edge, 1 = falling edge
// bit 3 0: CTRRST2, 0 = do not reset counter on CAP2 event
// bit 2 1: CAP2POL, 0 = rising edge, 1 = falling edge
// bit 1 0: CTRRST1, 0 = do not reset counter on CAP1 event
// bit 0 0: CAP1POL, 0 = rising edge, 1 = falling edge
// Enable desired eCAP interrupts
ECap1Regs.ECEINT.all = 0x0008;
// bit 15-8 0's: reserved
// bit 7 0: CTR=CMP, 0 = compare interrupt disabled
// bit 6 0: CTR=PRD, 0 = period interrupt disabled
// bit 5 0: CTROVF, 0 = overflow interrupt disabled
// bit 4 0: CEVT4, 0 = event 4 interrupt disabled
// bit 3 1: CEVT3, 1 = event 3 interrupt enabled
// bit 2 0: CEVT2, 0 = event 2 interrupt disabled
// bit 1 0: CEVT1, 0 = event 1 interrupt disabled
// bit 0 0: reserved
}
__interrupt void ecap1_isr(void);
void setup_led_GPIO(void);
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
//Predefine SPID_isr interrupt function
__interrupt void SPIB_isr(void);
void serialRXA(serial_t *s, char data);
//Predefine the init_eQEPs function
void init_eQEPs(void);
//predefine setupSpib
void setupSpib(void);
//Predefine the SetEPWM2 functions
void setEPWM2A(float);
void setEPWM2B(float);
void setEPWM8A_RCServo(float angle);
float readEncLeft(void); //initialize encoder reading float values
float readEncRight(void);
float scaling_float = 4095.0/3.0;
float footRatio = 0.08532423208; // = 1/11.72
float LeftWheel = 0; //Wheel travel distance in rads
float RightWheel = 0;
float LeftWheel_feet = 0; //Wheel travel distance in feet
float RightWheel_feet = 0;
float uLeft = 5.0; //Create two float variables uLeft and uRight that will be used as your control effort variables.
float uRight = 5.0;
float VRight = 0; //initialize velocity variables
float VLeft = 0;
//PI controller set K values
float Kp = 3;
float Ki = 25.5;
//Initialize Control Algorithm variables for equations Left & Right
//Left
float PosLeft_K = 0; //left wheel position, K is current
float PosLeft_K_1 = 0; //K_1 = previous wheel position
float ek_Left = 0;
float Ik_Left = 0;
float uk_Left = 0;
float ek_Left_1 = 0;
float Ik_Left_1 = 0;
//Right
float PosRight_K = 0; //right wheel position, K is current
float PosRight_K_1 = 0;
float ek_Right = 0;
float Ik_Right = 0;
float uk_Right = 0;
float ek_Right_1 = 0;
float Ik_Right_1 = 0;
//Vref Set value --> what the wheels are trying to achieve
float Vref = 1.0; //reference velocity
float Kp_turn = 3.0;
float ek_turn = 0;
float e_turn = 0;
float turn = 0;
float e_Left = 0;
float e_Right = 0;
float ki = 0;
float kd = 0;
////Predefine LAB 5 --> MPU Predefine
//INstered for the SPIB_isr interrupt function
int16_t spivalue1 = 0; // Read first 16 bit value off RX FIFO.
int16_t spivalue2 = 0; // Read second 16 bit value off RX FIFO.
int16_t spivalue3 = 0;
int16_t servo_countup = 0;
float RC1 = 0.0;
float RC2 = 0.0;
float RC1_voltage = 0.0;
float RC2_voltage = 0.0;
//Predefine Exercise 4
int16_t acceleration_x = 0;
int16_t acceleration_y = 0;
int16_t acceleration_z = 0;
int16_t temp = 0;
int16_t gyro_x = 0;
int16_t gyro_y = 0;
int16_t gyro_z = 0;
float acceleration_x_g = 0.0;
float acceleration_y_g = 0.0;
float acceleration_z_g = 0.0;
float gyro_x_g = 0.0;
float gyro_y_g = 0.0;
float gyro_z_g = 0.0;
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
float Left_angle = 70.0;
float Right_angle = -70.0;
void main(void) {
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
setup_led_GPIO();
//ULTRASONIC SENSOR
//---------------------------------------------------------
// Trigger pin for HC-SR04
GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO0 = 1;
// Echo pin for HC-SR04
EALLOW;
InputXbarRegs.INPUT7SELECT = 19; // Set eCAP1 source to GPIO-pin
EDIS;
GPIO_SetupPinOptions(19, GPIO_INPUT, GPIO_ASYNC);
//GPIO_SetupPinOptions(19, GPIO_INPUT, GPIO_PULLUP);
InitECapture();
//---------------------------------------------------------
//RC SERVO
//PWM -- EPWM8A Right RC Motor
GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1); //GPIO PinName 14 from PinMUX table, CPU, Mux Index from PinMUX Table for EPWM8A index is 1
// Blue LED on LuanchPad
GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO31 = 1;
// Red LED on LaunchPad
GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO34 = 1;
// LED1 and PWM Pin
GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;
// LED2
GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
// LED3
GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
// LED4
GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
// LED5
GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
// LED6
GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;
// LED7
GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;
// LED8
GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;
// LED9
GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;
// LED10
GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;
// LED11
GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
// LED12
GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
// LED13
GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;
// LED14
GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;
// LED15
GPIO_SetupPinMux(159, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(159, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;
// LED16
GPIO_SetupPinMux(160, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(160, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;
//WIZNET Reset
GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO0 = 1;
//ESP8266 Reset
GPIO_SetupPinMux(1, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(1, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO1 = 1;
// //SPIRAM CS Chip Select
// GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
// GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
// GpioDataRegs.GPASET.bit.GPIO19 = 1;
//DRV8874 #1 DIR Direction
GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO29 = 1;
//DRV8874 #2 DIR Direction
GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO32 = 1;
//DAN28027 CS Chip Select
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO9 = 1;
//MPU9250 CS Chip Select
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
//WIZNET CS Chip Select
GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDSET.bit.GPIO125 = 1;
//PushButton 1
GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(4, GPIO_INPUT, GPIO_PULLUP);
//PushButton 2
GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(5, GPIO_INPUT, GPIO_PULLUP);
//PushButton 3
GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(6, GPIO_INPUT, GPIO_PULLUP);
//PushButton 4
GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(7, GPIO_INPUT, GPIO_PULLUP);
//Joy Stick Pushbutton
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);
//PWM1 -- EPWM2A Right Motor
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //GPIO PinName 2 from PinMUX table, CPU, Mux Index from PinMUX Table for EPWM2A index is 1
//PWM2 -- EPWM2B Left Motor
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //GPIO PinName 3 from PinMUX table, CPU, Mux Index from PinMUX Table for EPWM2B index is 1
//PWM3 -- EPWM9A Buzzer
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5); //GPIO PinName 16 from PinMUX table, CPU, Mux Index from PinMUX Table for EPWM9A index is 1
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
// Initialize the PIE control registers to their default state.
// The default state is all PIE interrupts disabled and flags
// are cleared.
// This function is found in the F2837xD_PieCtrl.c file.
InitPieCtrl();
// Disable CPU interrupts and clear all CPU interrupt flags:
IER = 0x0000;
IFR = 0x0000;
// Initialize the PIE vector table with pointers to the shell Interrupt
// Service Routines (ISR).
// This will populate the entire table, even if the interrupt
// is not used in this example. This is useful for debug purposes.
// The shell ISR routines are found in F2837xD_DefaultIsr.c.
// This function is found in F2837xD_PieVect.c.
InitPieVectTable();
// Interrupts that are used in this example are re-mapped to
// ISR functions found within this project
EALLOW; // This is needed to write to EALLOW protected registers
PieVectTable.TIMER0_INT = &cpu_timer0_isr;
PieVectTable.TIMER1_INT = &cpu_timer1_isr;
PieVectTable.TIMER2_INT = &cpu_timer2_isr;
PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.ECAP1_INT = &ecap1_isr;
PieVectTable.SPIB_RX_INT = &SPIB_isr; //SPIB @ the SPIB_isr interrupt function
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
EDIS; // This is needed to disable write to EALLOW protected registers
// Initialize the CpuTimers Device Peripheral. This function can be found in F2837xD_CpuTimers.c
InitCpuTimers();
// Configure CPU-Timer 0, 1, and 2 to interrupt every second: // 200MHz CPU Freq, 1 second Period (in uSeconds)
ConfigCpuTimer(&CpuTimer0, 200, 1000);
ConfigCpuTimer(&CpuTimer1, 200, 1000);
ConfigCpuTimer(&CpuTimer2, 200, 4000); //set one of the unused CPU timer interrupts to timeout every 4 milliseconds
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serial(&SerialA,115200,serialRXA);
// init_serial(&SerialC,115200,serialRXC);
// init_serial(&SerialD,115200,serialRXD);
//Initialization Worker Functions
setupSpib(); //SPIB initialization function
init_eQEPs(); //eQEPS initialization function
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// EPWM2A/B Register Options
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
EPwm2Regs.TBCTL.bit.FREE_SOFT = 0x2; //Free Soft is in Free run mode, 0x2 from data sheet (1x which is either of 11 (3), 10 (2))
EPwm2Regs.TBCTL.bit.CTRMODE = 0x0; //Counter mode, 0x0 from data sheet sets it to count up
EPwm2Regs.TBCTL.bit.PHSEN = 0x0; //Phase loading disabled by turning it to 0x0 from data sheet for TBCTL
EPwm2Regs.TBCTL.bit.CLKDIV = 0x0; //Divide the clock by value of 1 (0x1)
EPwm2Regs.TBCTR = 0x0; //TBCTR is a counter and doesn't have bits so don't put .bit it counts 0-15 (16 bits)
EPwm2Regs.TBPRD = 2500; //Remember the clock source that the TBCTR register is counting is 50M Hz, and we want to make the count freq 20K Hz (50ms), convert using ratio of 2500
EPwm2Regs.CMPA.bit.CMPA = 600; //CompareA Register set to zero initially, this will control the width of the duty cycle for square waves of EPWMxA
EPwm2Regs.CMPB.bit.CMPB = 2000; //CompareB Register set to zero initially, this will control the width of the duty cycle for square waves of EPWMxA
EPwm2Regs.AQCTLA.bit.CAU = 0x1; //Signal is cleared when CMPA is reached
EPwm2Regs.AQCTLB.bit.CBU = 0x1; //Signal is cleared when CMPB is reached
EPwm2Regs.AQCTLA.bit.ZRO = 0x2; //Sets the pin when TBCTR is zero
EPwm2Regs.AQCTLB.bit.ZRO = 0x2; //Sets the pin when TBCTR is zero
EPwm2Regs.TBPHS.bit.TBPHS = 0; //Time based counter is not loaded with the phase
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// EPWM9A Register Options
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
EPwm9Regs.TBCTL.bit.FREE_SOFT = 0x2; //Free Soft is in Free run mode, 0x2 from data sheet (1x which is either of 11 (3), 10 (2))
EPwm9Regs.TBCTL.bit.CTRMODE = 0x0; //Counter mode, 0x0 from data sheet sets it to count up
EPwm9Regs.TBCTL.bit.PHSEN = 0x0; //Phase loading disabled by turning it to 0x0 from data sheet for TBCTL
EPwm9Regs.TBCTL.bit.CLKDIV = 0x1; //Divide the clock by value of 1 (0x1)
EPwm9Regs.TBCTR = 0x0; //TBCTR is a counter and doesn't have bits so don't put .bit it counts 0-15 (16 bits)
EPwm9Regs.TBPRD = 2500; //Remember the clock source that the TBCTR register is counting is 50M Hz, and we want to make the count freq 20K Hz (50ms), convert using ratio of 2500
//EPwm9Regs.CMPA.bit.CMPA = 0; //CompareA Register set to zero initially, this will control the width of the duty cycle for square waves of EPWMxA
EPwm9Regs.AQCTLA.bit.CAU = 0x0; //Signal is cleared when CMPA is reached
EPwm9Regs.AQCTLA.bit.ZRO = 0x3; //0x3 toggle, 50% duty cylce
EPwm9Regs.TBPHS.bit.TBPHS = 0; //Time based counter is not loaded with the phase
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// EPWM8A/B Register Options --- RC Servos -- A=
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
EPwm8Regs.TBCTL.bit.FREE_SOFT = 0x2; //Free Soft is in Free run mode, 0x2 from data sheet (1x which is either of 11 (3), 10 (2))
EPwm8Regs.TBCTL.bit.CTRMODE = 0x0; //Counter mode, 0x0 from data sheet sets it to count up
EPwm8Regs.TBCTL.bit.PHSEN = 0x0; //Phase loading disabled by turning it to 0x0 from data sheet for TBCTL
EPwm8Regs.TBCTL.bit.CLKDIV = 0x5; //Divide the clock by value of 32 (101 from CLKDIV datasheet) --> 50MHz / 32 = 1.5625 MHz / 50Hz = 31250
EPwm8Regs.TBCTR = 0x0; //TBCTR is a counter and doesn't have bits so don't put .bit it counts 0-15 (16 bits)
EPwm8Regs.TBPRD = 31250; //Remember the clock source that the TBCTR register is counting is 50M Hz, and we want to make the count freq 50 Hz (20 ms), convert using ratio of 50Mhz/32 CLKDIV / 50Hz = 31250 ratio
EPwm8Regs.CMPA.bit.CMPA = 2500; //CompareA Register set to zero initially, this will control the width of the duty cycle for square waves of EPWMxA
EPwm8Regs.CMPB.bit.CMPB = 2500; //CompareB Register set to zero initially, this will control the width of the duty cycle for square waves of EPWMxA
EPwm8Regs.AQCTLA.bit.CAU = 0x1; //Signal is cleared when CMPA is reached
EPwm8Regs.AQCTLB.bit.CBU = 0x1; //Signal is cleared when CMPB is reached
EPwm8Regs.AQCTLA.bit.ZRO = 0x2; //Sets the pin when TBCTR is zero
EPwm8Regs.AQCTLB.bit.ZRO = 0x2; //Sets the pin when TBCTR is zero
EPwm8Regs.TBPHS.bit.TBPHS = 0; //Time based counter is not loaded with the phase
//PIE Vector Initializations
IER |= M_INT1;
IER |= M_INT4; // INT4 which is connected to ECAP1-4 INT
IER |= M_INT6; //SPIB
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
// Enable ____ in the PIE: (Group PIEIER?) (interrupt INTx?)
PieCtrlRegs.PIEIER1.bit.INTx7 = 1; //Enable TINTO
PieCtrlRegs.PIEIER12.bit.INTx9 = 1; //Enable SWI
PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //Enable SPIB
PieCtrlRegs.PIEIER4.bit.INTx1 = 1; //Enable ECAP
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"Left Wheel Feet: %f Right Wheel Feet: %f \r\n",LeftWheel_feet,RightWheel_feet);
//serial_printf(&SerialA,"Accel X %.3f Accel Y %.3f Accel Z %.3f Gyro X %.3f Gyro Y %.3f Gyro Z %.3f \r\n",acceleration_x_g,acceleration_y_g,acceleration_z_g,gyro_x_g,gyro_y_g,gyro_z_g);
//serial_printf(&SerialA,"Left Wheel Vel: %f Right Wheel Vel: %f \r\n",VLeft,VRight);
serial_printf(&SerialA,"Left Wheel Pos: %.3f Right Wheel Pos: %.3f Accel X %.3f Accel Y %.3f Accel Z %.3f Gyro X %.3f Gyro Y %.3f Gyro Z %.3f \r\n",LeftWheel_feet,RightWheel_feet,acceleration_x_g,acceleration_y_g,acceleration_z_g,gyro_x_g,gyro_y_g,gyro_z_g);
UARTPrint = 0;
}
}
}
// SWI_isr, Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {
// These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
// making it lower priority than all other Hardware interrupts.
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP"); // Wait one cycle
EINT; // Clear INTM to enable interrupts
// Insert SWI ISR Code here.......
numSWIcalls++;
DINT;
}
__interrupt void cpu_timer0_isr(void)
{
//------Buzzer Song ------------
//button 1
// start_song = 0;
// if ((GpioDataRegs.GPADAT.bit.GPIO6)== 0) {
// start_song = 1;
// }
// if (start_song == 1) {
// EPwm9Regs.TBPRD = song2array[countToSongLength];
// countToSongLength++;
// }
// if (countToSongLength == 95) {
// countToSongLength = 0;
// countToSongLength++;
// start_song = 1;
// }
// if ((GpioDataRegs.GPADAT.bit.GPIO7)== 0) {
// GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); //GPIO PinName 16 from PinMUX table, CPU, Mux Index from PinMUX Table for EPWM9A index is 1
// }
//increment timer interrupt count
CpuTimer0.InterruptCount++;
numTimer0calls++;
// //Exercise 4
// GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; //Clear GPIO66
// //Make sure to set SpibRegs.SPIFFRX.bit.RXFFIL to the correct value so that the SPIB
// //interrupt function will be called when the SPI transmission from the master to the slave and also from the
// //slave to the master is complete
// SpibRegs.SPIFFRX.bit.RXFFIL = 8; //0x10h = 8 bits for each sensor pair from datasheet : A RX FIFO interrupt request is generated when there are 16 words in the RX buffer
//
// SpibRegs.SPITXBUF = (0x8000 | 0x3A00); //0x8000 initiates reading, 0x3A00 is location of address we want to read
// //Write the next 7 values in the register
// // hex dec hex dec
// SpibRegs.SPITXBUF = 0x0000; // TO 3B (59) AND 3C (60) FOR ACCEL_X
// SpibRegs.SPITXBUF = 0x0000; // TO 3D (61) AND 3E (62) FOR ACCEL_Y
// SpibRegs.SPITXBUF = 0x0000; // TO 3F (63) AND 40 (64) FOR ACCEL_Z
// SpibRegs.SPITXBUF = 0x0000; // TO 41 (65) AND 42 (66) FOR TEMP_OUT
// SpibRegs.SPITXBUF = 0x0000; // TO 43 (67) AND 44 (68) FOR GYRO_X
// SpibRegs.SPITXBUF = 0x0000; // TO 45 (69) AND 46 (70) FOR GYRO_Y
// SpibRegs.SPITXBUF = 0x0000; // TO 47 (71) AND 48 (72) FOR GYRO_Z
//
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
// // Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
// //PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //Enable SPIB
//
// //If the timer interrupt count is divisible by 20, print (print every 100ms)
// // 20,000 us timer, 20,000/20 = 1,000 us = 1ms
// if ((numTimer0calls % 200) == 0){
//
// UARTPrint = 1;
// }
if ((numTimer0calls%250) == 0) {
displayLEDletter(LEDdisplaynum);
LEDdisplaynum++;
if (LEDdisplaynum == 0xFFFF) { // prevent roll over exception
LEDdisplaynum = 0;
}
}
}
Uint32 numTimer1calls = 0;
int16_t obj_detected_flag = 0;
Uint32 time_obj_detected = 0;
int16_t turn_time = 0;
float speed_Vref = 0.5;
float echo_distance_Right = 0;
float echo_distance_Left = 0;
float echo_distance_Front = 0;
Uint32 original_time_obj_detected = 0;
float obj_det_speed = 1.0;
float obj_det_turn = 1.0;
int16_t running_algorithm_flag = 0;
int16_t Left_flag = 0;
int16_t Right_flag = 0;
int16_t Turn_flag = 0;
int16_t End_flag = 0;
//dancing
uint16_t dance_count = 0;
uint16_t dance_flag = 0;
uint16_t linear_direction_flag = 1;
uint16_t turn_direction_flag = 1;
uint16_t linear_direction_count = 0;
uint16_t turn_direction_count = 0;
__interrupt void cpu_timer1_isr(void)
{
numTimer1calls++;
CpuTimer1.InterruptCount++;
// Trigger HC-SR04
hc_sr04_trigger();
// if (echo_average > 15.0) {
// Vref = 1.0;
// turn = 0.0;
// }
//Vref = obj_det_speed;
//turn = 0.0;
setEPWM8A_RCServo(0.0);
// if ((GpioDataRegs.GPADAT.bit.GPIO4)== 0) {
// setEPWM8A_RCServo(Left_angle);
// }
// if ((GpioDataRegs.GPADAT.bit.GPIO5)== 0) {
// setEPWM8A_RCServo(Right_angle);
// }
//if an object is 10.0cm away, object is considered deteced
if(echo_average < 18.0 && running_algorithm_flag == 0) {
obj_detected_flag = 1;
time_obj_detected = CpuTimer1.InterruptCount;
}
if (obj_detected_flag == 1) {
//running_algorithm_flag = 1; //The algorithm is now running, don't back up
//For 500 counts of object being detected, back up and store echo_distance
if (abs(CpuTimer1.InterruptCount - time_obj_detected) == 500 ) {
Left_flag = 1; //this flag makes the ultraosnic look left
running_algorithm_flag = 1; //start running algorithm
}
if (abs(CpuTimer1.InterruptCount - time_obj_detected) == 1000 ) {
Left_flag = 0; //clear
Right_flag = 1; //this flag makes the ultraosnic look right
}
if (abs(CpuTimer1.InterruptCount - time_obj_detected) == 1500 ) {
Right_flag = 0; //clear
Turn_flag = 1; ////this flag makes initiates the turning portion
}
if (abs(CpuTimer1.InterruptCount - time_obj_detected) == 2700 ) {
Turn_flag = 0;
End_flag = 1; //this resets the algorithm
running_algorithm_flag = 0; //done running algorithm
}
// if (abs(CpuTimer1.InterruptCount - time_obj_detected) > 7000 ) {
// End_flag = 0;
// }
if (abs(CpuTimer1.InterruptCount - time_obj_detected) < 500 ) {
running_algorithm_flag = 1;
echo_distance_Front = echo_distance;
turn = 0.0;
Vref = -obj_det_speed;
}
//For 1000 counts (after backing up) stop moving, turn the Servo to the Left & store the distance
if (Left_flag == 1) {
Vref = 0.0;
turn = 0.0;
setEPWM8A_RCServo(Left_angle);
echo_distance_Left = echo_distance;
//running_algorithm_flag = 1;
}
//For 1000 counts (after looking left) stay still, turn the Servo to the Right & store the distance
if (Right_flag == 1) {
Vref = 0.0;
turn = 0.0;
setEPWM8A_RCServo(Right_angle);
echo_distance_Right = echo_distance;
//running_algorithm_flag = 1;
}
if (Turn_flag == 1) {
if (echo_distance_Right >= echo_distance_Left) {
Vref = 0.0;
turn = -obj_det_turn; //turn right
//running_algorithm_flag = 1;
}
if (echo_distance_Left >= echo_distance_Right) {
Vref = 0.0;
turn = obj_det_turn; //turn left
//running_algorithm_flag = 1;
}
// if (echo_distance_Front >= echo_distance_Right) {
// Vref = 0.0;
// turn = -obj_det_turn;
// }
// else {
// Vref = 0.0;
// turn = -obj_det_turn;
// //running_algorithm_flag = 0;
//// Vref = obj_det_speed;
//// turn = 0.0;
//// End_flag = 0;
//// Right_flag = 0;
//// Left_flag = 0;
//// Turn_flag = 0;
// }
}
if (End_flag == 1) {
turn = 0.0;
Vref = obj_det_speed;
obj_detected_flag = 0;
//running_algorithm_flag = 0;
End_flag = 0;
}
//running_algorithm_flag = 0;
}
// if (Turn_flag == 1) {
// if (echo_distance_Front >= echo_distance_Left) {
// Vref = 0.0;
// turn = obj_det_turn;
// }
//// if (echo_distance_Front >= echo_distance_Right) {
//// Vref = 0.0;
//// turn = -obj_det_turn;
//// }
// else {
// Vref = 0.0;
// turn = -obj_det_turn;
//
//// Vref = obj_det_speed;
//// turn = 0.0;
//// End_flag = 0;
//// Right_flag = 0;
//// Left_flag = 0;
//// Turn_flag = 0;
// }
// }
...
This file has been truncated, please download it to see its full contents.
Comments