Hardware components | ||||||
 |
| × | 2 | |||
 |
| × | 1 | |||
 |
| × | 2 | |||
| × | 1 | ||||
 |
| × | 1 | |||
| × | 1 | ||||
Software apps and online services | ||||||
 |
| |||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
 |
| |||||
For the ME 461 Computer Control of Mechanical Systems final project, we modified our robot car to launch candies when it sees us. The robot car will also return to a refilling station that will add a candy to the launching mechanism of the car.
The mechanism uses a spring, barrel, gear, and pinion. The motor will spin which rotates the gear. This then makes the pinion move, and once the teeth of thegear aren't engaged with the pinion, the skittle is launched from the spring being compressed. The gear not having a full circle of teeth allows the mechanism to be launched. We also used some motor control to only have motor spin one rotation once launcher is aimed at a person.
The car uses dead reckoning with its wheel encoders and an algorithm for pose, x-position, and y-position developed in ME 461 lab 6 to know its general location in the robot car course. The robot also utilizes a function xy_control from SE 423 lab code to send the robot to a certain xy coordinate searching the course for someone to shoot at.
When the OpenMV camera on the front of the robot catches a large blob of green color, it stops roaming and centers on the blob (the person to shoot at) by turning. When centered on the green blob for long enough, the robot car sets a GPIO pin high on the car's F28379D to signal a launch command. Another F28379D, connected to the original F28379D's GPIO pin, will command a launching motor to turn one rotation when that GPIO pin is received high. The motor is connected to a gear with all but a few teeth removed, so it becomes disengaged with a rack at a section of the rotation. The rack is connected to a spring that rapidly extends when the gear disengages, pulling a plunger that launches the candy.
When the candy is launched the robot car will return to a location far from, but straight in front of, an april tag. The car will then attempt to turn directly toward the april tag. If the tag is located by the camera, the robot will center on the tag and drive towards it, adjusting left or right to remain centered on the tag. If the robot misses the april tag on the first search, it will spin until it finds the tag and drive towards the tag.
When the april tag is close enough, the robot car assumes it is under the refill station, and waits a few seconds for dispense. At this location, the car also zeros its x-y orientation and pose, resetting the dead reckoning system. A video summary of the project is attached below. A state machine diagram is also attached below.
Once the robot has shot a skittle, it will need to be refilled. The robot's state switches from looking for green to looking for the april tag. The refilling station, which the robot will stop directly underneath, is mounted near the april tag. It consists of one servo, three PLA parts, one to hold the skittles, one to hold the servo motor and mount, and a back cover to the skittle holding piece. The servo arm inserts directly into the piece holding the skittles, and the entire refill station is controlled by another green board and F28379D combination. A distance sensor was added to the green board, and is mounted directly underneath the refill station. This sensor is used more as a motion/proximity sensor, as the raw value from it when it detects an object changes. The code is set to rotate the refill station by 21 degrees each time the sensor changes significantly in value. A short delay in between detecting the new value and the servo rotating was added. This helps to time the dispensing of one skittle directly into the robot's launcher once the robot has stopped moving. To ensure only one skittle gets dispensed at a time, a third state in the code prevents going in between the first 2 states until after the robot has left. This is done by setting the value of the distance sensor low enough for it not to detect the robot car anymore. The refilling station can hold 8 skittles only within 0 to 180 degrees from each end, at which the servo must rotate back to its initial position.
During robot demonstration, the robot car successful roamed, saw the green blob and shot a candy. The car also found the april tag and drove towards it. However, the car did not line up correctly under the refill station, this is because the launcher, funnel, and launcher motor add weight away from the car's drive wheels, causing significant wheel slippage and therefore inaccuracies in the dead reckoning system. These inaccuracies caused the car not to move to a point directly in front of the april tag, meaning the robot drove in at an angle. The robot was also programmed to stop too close to the april tag, so the camera sometimes lost the april tag in the frame, causing the robot never to reset the dead reckoning system and switch states back to roaming.
OpenMV Blob & AprilTag Finder
MicroPythonThis code was flashed onto the team's OpenMV camera's SD card. This runs whenever it is powered. It is currently set to look for a neon green blob as well as an AprilTag. It communicates with the F28379D the blob's area, centroid x-coordinate, and centroid y-coordinate. For the AprilTag it send the z-distance (distance forward and back from camera), centroid x-coordinate, and centroid y-coordinate.
This code was made on OpenMV's IDE which is based on MicroPython.
This code was made on OpenMV's IDE which is based on MicroPython.
# Blob_AT_Finder - By: joshu - Wed Dec 14 2022
import image, sensor, ustruct, pyb, time
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
#sensor.set_framesize(sensor.QVGA) # use QVGA 320*240
sensor.set_framesize(sensor.QQVGA) # use QQVGA 160*120
sensor.set_vflip(True) # Flips the image vertically
sensor.set_hmirror(True) # Mirrors the image horizontally
sensor.set_auto_gain(False, gain_db=11.4801) # must be turned off for color tracking
sensor.set_auto_whitebal(False,[60.2071, 60.5557, 67.1094]) # must be turned off for color tracking
#print(sensor.get_rgb_gain_db())
uart = pyb.UART(3)
uart.init(115200, bits=8, parity=None)
threshold1 = (26, 41, 13, 127, 17, 71) # Orange
threshold2 = (8, 86, -112, -17, -23, 60) # Green
# Packets to Send
blob_packet = '<fff'
# ~~~~~~~~~APRIL TAG SETUP~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
# The apriltag code supports up to 6 tag families which can be processed at the same time.
# Returned tag objects will have their tag family and id within the tag family.
tag_families = 0
tag_families |= image.TAG16H5 # comment out to disable this family
tag_families |= image.TAG25H7 # comment out to disable this family
tag_families |= image.TAG25H9 # comment out to disable this family
tag_families |= image.TAG36H10 # comment out to disable this family
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
tag_families |= image.ARTOOLKIT # comment out to disable this family
# What's the difference between tag families? Well, for example, the TAG16H5 family is effectively
# a 4x4 square tag. So, this means it can be seen at a longer distance than a TAG36H11 tag which
# is a 6x6 square tag. However, the lower H value (H5 versus H11) means that the false positve
# rate for the 4x4 tag is much, much, much, higher than the 6x6 tag. So, unless you have a
# reason to use the other tags families just use TAG36H11 which is the default family.
def family_name(tag):
if(tag.family() == image.TAG16H5):
return "TAG16H5"
if(tag.family() == image.TAG25H7):
return "TAG25H7"
if(tag.family() == image.TAG25H9):
return "TAG25H9"
if(tag.family() == image.TAG36H10):
return "TAG36H10"
if(tag.family() == image.TAG36H11):
return "TAG36H11"
if(tag.family() == image.ARTOOLKIT):
return "ARTOOLKIT"
# ~~~~~~~~~APRIL TAG SETUP~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Setup RED LED for easier debugging
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
clock = time.clock() # Create a clock object to track the FPS.
framecount = 0
toggle = 0
offcount = 0
while True:
clock.tick() # Update the FPS clock.
if framecount % 2 == 0:
if toggle == 0:
blue_led.on()
toggle = 1
offcount = 0
else:
blue_led.off()
offcount = offcount + 1
if offcount == 20:
toggle = 0
offcount = 0
img = sensor.snapshot()
# roi is left, top, width, height
#blobs1 = img.find_blobs([threshold2], roi=(0,120,320,120), pixels_threshold=50, area_threshold=20, merge=True)
blobs1 = img.find_blobs([threshold2], pixels_threshold=50, area_threshold=20, merge=True)
if blobs1:
blob1_sort = sorted(blobs1, key = lambda b: b.pixels(), reverse=True)
#blob_largest = blob1_sort[:3]
blobs_found = len(blob1_sort)
msg = "**".encode()
uart.write(msg)
if (blobs_found > 0):
b = blob1_sort[0]
a = float(b.area())
x_cnt = float(b.cx())
y_cnt = float(b.cy())
#print("x: ", x_cnt)
#print("y: ", y_cnt)
#img.draw_rectangle(b[0:4]) # rect on x,y,w,h
#img.draw_cross(b.cx(), b.cy())
else:
a = 0.0
x_cnt = 0.0
y_cnt = 0.0
# Send the blob area and centroids over UART
b = ustruct.pack(blob_packet, a, x_cnt, y_cnt)
uart.write(b)
else: # nothing found
msg = "**".encode()
uart.write(msg)
b = ustruct.pack(blob_packet, 0.0, 0.0, 0.0)
uart.write(b)
#b = ustruct.pack(blob_packet, 0.0, 0.0, 0.0)
#uart.write(b)
#b = ustruct.pack(blob_packet, 0.0, 0.0, 0.0)
#uart.write(b)
tags = img.find_apriltags(families=tag_families)
#print(len(tags))
if (len(tags) > 0):
msg = '*!'.encode()
uart.write(msg)
#img.draw_rectangle(tags[0].rect(), color = (255, 0, 0))
#img.draw_cross(tags[0].cx(), tags[0].cy(), color = (0, 255, 0))
#print_args = (tags[0].z_translation(), tags[0].cx(), tags[0].cy())
#print("z dist: %f , cx: %f , cy: %f" % print_args)
b = ustruct.pack(blob_packet, tags[0].z_translation(), tags[0].cx(), tags[0].cy())
uart.write(b)
else:
msg = '*!'.encode()
uart.write(msg)
b = ustruct.pack(blob_packet, 0.0, 0.0, 0.0)
uart.write(b)
#roi is left, top, width, height
#print(clock.fps()) # Note: OpenMV Cam runs about half as fast when connected
//#############################################################################
// 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
// The Launchpad's CPU Frequency set to 200 you should not change this value
#define LAUNCHPAD_CPU_FREQUENCY 200
// 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);
__interrupt void ADCA_ISR (void);// initialization of ADCA interrupt function to be able to sample the distance sensor
void setEPWM8A_RCServo(float angle);//change the angle of one of the servo motors on our board
//void DistanceCount(void);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
int16_t servos = 1; //declare as 1 to be able to begin counting up servo angle
float rotate = -90.0;//will be passed on to both set_EPWM8()_RCServo functions
int myStateVar = 0;//keeps track of the current state
float adca0result = 0;//declare new variable to store reading from adca0 from distance sensor
uint32_t ADCInterruptCount = 0;//ADC function interrupt count, from lab 4
uint32_t delayCounter = 0;//sets a timed delay based on how many times timer 0 interrupt is triggered in the state machine code state 1
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
// Blue LED on LaunchPad
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, 1);// since the joystick is not being used, GPIO8 goes to EPWM5, which will be used for conversions
GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);
// 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.SCIB_RX_INT = &RXBINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.ADCA1_INT = &ADCA_ISR;//Re-maps ADCA1_INT interrupt to ISR function ADCA_ISR
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 given period:
// 200MHz CPU Freq, Period (in uSeconds)
ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 10000);
ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 20000);
ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 5000);
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serialSCIA(&SerialA,115200);
// init_serialSCIB(&SerialB,115200);
// init_serialSCIC(&SerialC,115200);
// init_serialSCID(&SerialD,115200);
EALLOW;
//Initializations for EPWM5 from lab 3, no significant changes made
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 1;
EPwm5Regs.TBCTR = 0x0; // Clear counter
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
EPwm5Regs.TBPRD = 5000; // Set Period to 1ms sample. Input clock is 50MHz.
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
EALLOW; // Below are protected registers
GpioCtrlRegs.GPAPUD.bit.GPIO14 = 1; // For EPWM8A, initialization from lab 3
EDIS;
GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);//Changes GPIO14 using pin Mux to EPWM8A, which will be used to power the servo
//Additional initializations for EPWM8A only from lab 3, no significant changes made
EPwm8Regs.TBCTL.bit.CTRMODE = 0; // set mode to count up mode page 25 shortened document
EPwm8Regs.TBCTL.bit.FREE_SOFT = 2; //this could be 2 or 3 to set free run because 1x: page 23
EPwm8Regs.TBCTL.bit.PHSEN = 0; //disable phase loading page 25
EPwm8Regs.TBCTL.bit.CLKDIV = 4; //page 24 prescale value of 1 which clock divides by 16, setting to 4 allows us to divide by 16
EPwm8Regs.TBCTR = 0; //start the timer at 0. page 26
EPwm8Regs.TBPRD = 62500;//(50mil*0.02/CLKDIV) largest number under 65535 for a 20Hz carrier frequency with EPWM8. gives largest possible range of motor control angles
//Initializations for EPWM8A, from lab 3 code
EPwm8Regs.CMPA.bit.CMPA =5000;//duty cycle initialized at 8 percent (5000/62500 = 0.08)
EPwm8Regs.AQCTLA.bit.CAU = 1;//clear
EPwm8Regs.AQCTLA.bit.ZRO = 2;// sets the pin when TBCTR = 0, forces EPwm high. pg 36
EALLOW;
//write configurations for ADCA, from lab 4
AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
//Set pulse positions to late
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
//power up the ADCs
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//Select the channels to convert and end of conversion flag
//Many statements commented out, To be used when using ADCA or ADCB
//ADCA
AdcaRegs.ADCSOC0CTL.bit.CHSEL =3; //SOC0 will convert Channel you choose Does not have to be A0, using A3 from the joystick to read distance sensor, could also have used A2
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCA1, only one value so ADCA0 is the only value converted
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
// Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
// which is connected to CPU-Timer 1, and CPU int 14, which is connected
// to CPU-Timer 2: int 12 is for the SWI.
IER |= M_INT1;
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;// enables ADCA1 interrupt function 1.1, taken from lab 4 code
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
setEPWM8A_RCServo(rotate);// initialize servo rotation at -90 degree position to avoid unwanted movement when it is first powered on
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
serial_printf(&SerialA,"Num Timer2:%ld Num SerialRX: %ld DistanceSensor: %.2f\r\n",CpuTimer2.InterruptCount,numRXA, adca0result);// allows to see the raw distance sensor values in real time in tera term
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;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
switch(myStateVar){
case 0:// detect distance initially
if (adca0result > 1400){// from testing, raw distance sensor result was around 1500 when the robot car passed underneath, should trigger case 1
myStateVar = 1;// go to case 1
delayCounter = 0;// sets a delay variable so the refill station doesn't rotate right away
}
break;
case 1:// rotate and dispense skittles
delayCounter++;
if (delayCounter>=250) {//after 2.5 seconds (250 times the case is called multiplied by the period of 0.01s), dispense one skittle, allows the robot to stop completely
rotate = rotate + 21.0;// increase position by 21.0 degrees, changed from 22.5 degrees to correct overshooting
if (rotate > 90.0){//set max at 90 degrees, at which reset to -90 (initial position)
rotate = -90.0;
}
setEPWM8A_RCServo(rotate);// pass rotate value to epwm8 to control servo
myStateVar = 2;// go to leaving refill station case
break;
case 2://leaving, additional case ensuring no more skittles are dispensed if the car is still underneath refill
if (adca0result < 600){// do not start measuring proximity (state 0) until the robot car has left. value from sensor to ground was about 400, so a 600 lower bound should only trigger once car has left
myStateVar = 0;// switch back to state 0 once the car has left, stay in state 2 if it has not
}
break;
}
}
CpuTimer0.InterruptCount++;
numTimer0calls++;
// if ((numTimer0calls%50) == 0) {
// PieCtrlRegs.PIEIFR12.bit.INTx9 = 1; // Manually cause the interrupt for the SWI
// }
if ((numTimer0calls%250) == 0) {
displayLEDletter(LEDdisplaynum);
LEDdisplaynum++;
if (LEDdisplaynum == 0xFFFF) { // prevent roll over exception
LEDdisplaynum = 0;
}
}
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
// Blink LaunchPad Blue LED
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
CpuTimer2.InterruptCount++;
if ((CpuTimer2.InterruptCount % 50) == 0) {
UARTPrint = 1;
}
}
//adca interrupt function from lab 4 with only the important lines kept for functionality
__interrupt void ADCA_ISR (void)
{
adca0result = AdcaResultRegs.ADCRESULT0;//take reading from ADCD0 and store as a float, represents raw value from the distance sensor
ADCInterruptCount++;//increases interrupt count by 1 every time the function is called
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
//EPWM8 function to control servos from lab 3
void setEPWM8A_RCServo(float angle){//EPWM8A function with local variable input "angle", void returns nothing
float dutycycle1 = 0;//similar to EPWM2 functions, this variable will be used to store the current duty cycle
float divide1 = 1900.0;//constant required to divide by for a duty cycle range of 4 to 12 percent between min and max angle values, modified slightly from lab 3 to get a more precise duty cycle
if(angle > 90)
{
angle=90;//set max value of angle to 90
}
if(angle < -90)
{
angle = -90;//set min value of angle to -90
}
dutycycle1 = 0.08 + angle/divide1;//dutycycle1 has a range of 4% to 12% depending on the value of angle
EPwm8Regs.CMPA.bit.CMPA = dutycycle1*EPwm8Regs.TBPRD;////sets CMPA on EPWM8 to the percent duty cycle computed above relative to TBPRD
}
//#############################################################################
// 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
// The Launchpad's CPU Frequency set to 200 you should not change this value
#define LAUNCHPAD_CPU_FREQUENCY 200
// 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);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
//_____________________________________________________________________________Imported from lab 6 to control bot__________________________________
// Lab 5 work
// Function predefinitions
__interrupt void SPIB_isr(void); // SPIB interrupt function
void setupSpib(void); // Helper function that houses SPIB initialization to make main() code cleaner
float numberToAcceleration(int16_t value); // Converts given sensor reading from (-32768, 32767) to (-4, 4)
float numberToRotVel(int16_t value); // Converts given sensor reading from (-32768, 32767) to (-250, 250)
// Variable Initializations
// L5E3: DAN global variables
uint16_t spivalue1 = 0;
uint16_t spivalue2 = 0;
float spivoltage1 = 0;
float spivoltage2 = 0;
uint16_t DANpwm1 = 0;
uint16_t DANpwm2 = 0;
int16_t flip1 = 1;
int16_t flip2 = 1;
uint16_t spivalue0 = 0;
// L5E4: MPU global variables
float temp;
float accelX;
float accelY;
float accelZ;
float gyroX;
float gyroY;
float gyroZ;
// Lab 6 Work predefinitions and initializations
//eQEPs
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
float leftWheelAngle;
float rightWheelAngle;
//ePWMs
void init_EPWMs(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
float uLeft = 5.0; // Initialize to 5 control effort (zero speed)
float uRight = 5.0;
//Velocity calculations
float posLeftk;
float posLeftk_1;
float posRightk;
float posRightk_1;
float vLeftk;
float vRightk;
float Vref = 0;
float Kp = 3.0;
float Ki = 25.0;
float errLeftk;
float errLeftk_1;
float errRightk;
float errRightk_1;
float ILeftk;
float ILeftk_1;
float IRightk;
float IRightk_1;
float turn = 0;
float Kp_turn = 3;
float errTurn;
// Excercise 5
float rWheel = 0.1955;
float width = 0.576;
float pose;
float thetaAvg;
float omegaAvg;
float x_vel;
float y_vel;
float x_coor;
float y_coor;
float x_vel_1;
float y_vel_1;
float x_coor_1;
float y_coor_1;
float leftWheelAngle_1;
float rightWheelAngle_1;
float omegaLeft;
float omegaRight;
uint16_t identifier = 0;
uint16_t counter = 0;
uint16_t busy = 0;
float angleCounter = 0;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
float x_ref;
float y_ref;
uint16_t attarget;
uint16_t state = 404;
uint16_t state_1;
uint16_t blobtimer;
uint16_t state_2;
uint16_t firsttimer;
uint16_t shoottimer;
float thetarel;
uint16_t waittime;
float variable1;
float variable2;
uint16_t apriltimer;
uint16_t locked;
uint16_t lockedtimer;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
extern uint16_t COMB_state;
extern float fromCAMvaluesThreshold1[];
extern float fromCAMvaluesThreshold2[];
__interrupt void SPIB_isr(void){
temp = SpibRegs.SPIRXBUF;
int16_t accelX_raw = SpibRegs.SPIRXBUF;
int16_t accelY_raw = SpibRegs.SPIRXBUF;
int16_t accelZ_raw = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
int16_t gyroX_raw = SpibRegs.SPIRXBUF;
int16_t gyroY_raw = SpibRegs.SPIRXBUF;
int16_t gyroZ_raw = SpibRegs.SPIRXBUF;
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //
accelX = numberToAcceleration(accelX_raw);
accelY = numberToAcceleration(accelY_raw);
accelZ = numberToAcceleration(accelZ_raw);
gyroX = numberToRotVel(gyroX_raw);
gyroY = numberToRotVel(gyroY_raw);
gyroZ = numberToRotVel(gyroZ_raw);
// Acknowledge code
SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1; // Clear Overflow flag just in case of an overflow
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1; // Clear RX FIFO Interrupt flag so next interrupt will happen
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6; // Acknowledge INT6 PIE interrupt
}
// L5E4: Converts acceleration sensor value to proper range
float numberToAcceleration(int16_t value) {
if (value < 0) {
return value * (4.0 / 32768.0);
}
return value * (4.0 / 32767.0);
}
// L5E4: Converts gyro sensor value to proper range
float numberToRotVel(int16_t value) {
if (value < 0) {
return value * (250.0 / 32768.0);
}
return value * (250.0 / 32767.0);
}
//_____________________________________________________________________________________________________________________________________________________
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
float min(float x, float y){
if (x > y){
return y;
} else if (x < y){
return x;
} else {
return x;
}
}
float my_atanf(float dy, float dx)
{
float ang;
if (fabsf(dy) <= 0.001F) {
if (dx >= 0.0F) {
ang = 0.0F;
} else {
ang = PI;
}
} else if (fabsf(dx) <= 0.001F) {
if (dy > 0.0F) {
ang = HALFPI;
} else {
ang = -HALFPI;
}
} else {
ang = atan2f(dy,dx);
}
return ang;
}
int xy_control(float *vref_forxy, float *turn_forxy,float turn_thres, float x_pos,float y_pos,float x_desired,float y_desired,float thetaabs,float target_radius,float target_radius_near)
{
float dx,dy,alpha;
float dist = 0.0F;
float dir;
float theta;
int target_near = 0;
float turnerror = 0;
// calculate theta (current heading) between -PI and PI
if (thetaabs > PI) {
theta = thetaabs - 2.0*PI*floorf((thetaabs+PI)/(2.0*PI));
} else if (thetaabs < -PI) {
theta = thetaabs - 2.0*PI*ceilf((thetaabs-PI)/(2.0*PI));
} else {
theta = thetaabs;
}
dx = x_desired - x_pos;
dy = y_desired - y_pos;
dist = sqrtf( dx*dx + dy*dy );
dir = 1.0F;
// calculate alpha (trajectory angle) between -PI and PI
alpha = my_atanf(dy,dx);
// calculate turn error
// turnerror = theta - alpha; old way using left hand coordinate system.
turnerror = alpha - theta;
// check for shortest path
if (fabsf(turnerror + 2.0*PI) < fabsf(turnerror)) turnerror += 2.0*PI;
else if (fabsf(turnerror - 2.0*PI) < fabsf(turnerror)) turnerror -= 2.0*PI;
if (dist < target_radius_near) {
target_near = 1;
// Arrived to the target's (X,Y)
if (dist < target_radius) {
dir = 0.0F;
turnerror = 0.0F;
} else {
// if we overshot target, we must change direction. This can cause the robot to bounce back and forth when
// remaining at a point.
if (fabsf(turnerror) > HALFPI) {
dir = -dir;
}
turnerror = 0;
}
} else {
target_near = 0;
}
// vref is 1 tile/sec; but slower when close to target.
*vref_forxy = dir*min(dist,0.4); // Change 0.5 for speed?
if (fabsf(*vref_forxy) > 0.0) {
// if robot 1 tile away from target use a scaled KP value.
// *turn_forxy = 0.5*turnerror;
*turn_forxy = (*vref_forxy*2)*turnerror;
} else {
// normally use a Kp gain of 2
*turn_forxy = 2*turnerror;
// *turn_forxy = 0.5*turnerror;
}
// This helps with unbalanced wheel slipping. If turn value greater than
// turn_thres then just spin in place
if (fabsf(*turn_forxy) > turn_thres) {
*vref_forxy = 0;
}
return(target_near);
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Final Project Work
long time = 0;
void statefunction(void){
// //read sensors
time++;
switch(state){
case 404:
if (state_1 != state){
firsttimer = 0;
}
displayLEDletter(7);
firsttimer++;
variable1 = readEncLeft();
variable2 = readEncRight();
Vref = 0;
turn = 0;
x_ref = 0;
y_ref = 0;
x_coor = 0;
y_coor = 0;
pose = 0;
state_1 = 404;
x_coor_1 = 0;
x_vel_1 = 0;
y_coor_1 = 0;
y_vel_1 = 0;
posLeftk = 0;
posRightk = 0;
vLeftk = 0;
vRightk = 0;
errTurn = 0;
errLeftk = 0;
errRightk = 0;
ILeftk = 0;
IRightk = 0;
uLeft = 0;
uRight = 0;
attarget = 0;
posLeftk_1 = 0;
posRightk_1 = 0;
leftWheelAngle_1 = 0;
rightWheelAngle_1 = 0;
errLeftk_1 = 0;
errRightk_1 = 0;
ILeftk_1 = 0;
IRightk_1 = 0;
x_vel_1 = 0;
y_vel_1 = 0;
x_coor_1 = 0;
y_coor_1 = 0;
if (firsttimer>=1000){
x_coor = 0;
y_coor = 0;
state = 0;
}
break;
case 0: //Move to center
displayLEDletter(0);
x_ref = -2;
y_ref = 2;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 1;
attarget = 0;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 1:
displayLEDletter(0);
if (state_1 != state){
angleCounter = pose - (PI*1.5); //Do 360 looking for people
state_1 = state;
}
if (pose > angleCounter){
turn = -0.5;
} else {
state_1 = state;
state = 2;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 2:
displayLEDletter(0);
x_ref = -6;
y_ref = 2;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 3;
attarget = 0;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 3:
displayLEDletter(0);
if (state_1 != state){
angleCounter = pose - (PI*2.5); //Do 360 looking for people
state_1 = state;
}
if (pose > angleCounter){
turn = -0.5;
} else {
state_1 = state;
state = 4;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 4:
displayLEDletter(0);
x_ref = -6;
y_ref = 6;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 5;
attarget = 0;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 5:
displayLEDletter(0);
if (state_1 != state){
angleCounter = pose - (PI*2.5); //Do 360 looking for people
state_1 = state;
}
if (pose > angleCounter){
turn = -0.5;
} else {
state_1 = state;
state = 6;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 6:
displayLEDletter(0);
x_ref = -2;
y_ref = 6;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 7;
attarget = 0;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 7:
displayLEDletter(0);
if (state_1 != state){
angleCounter = pose - (PI*2.5); //Do 360 looking for people
state_1 = state;
}
if (pose > angleCounter){
turn = -0.5;
} else {
state_1 = state;
state = 8;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 8: //Move to center
displayLEDletter(0);
x_ref = -2;
y_ref = 2;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 9;
attarget = 0;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 9:
displayLEDletter(0);
if (state_1 != state){
angleCounter = pose - (PI*2.5); //Do 360 looking for people
state_1 = state;
}
if (pose > angleCounter){
turn = -0.5;
} else {
state_1 = state;
state = 2;
}
if (fromCAMvaluesThreshold1[0]!=0){
state_1 = state;
state = 10;
}
break;
case 10: //finds person, centers on blob: if more of color on one side turn more that way? when in center move to 20?
displayLEDletter(1);
Vref = 0;
turn = 0;
if (state_1 != state){
blobtimer = 0;
state_2 = state_1;
state_1 = state;
}
if (fromCAMvaluesThreshold1[0]==0){
state = state_2;
state_1 = state_2;
} else {
if (fromCAMvaluesThreshold1[1]<70){
// turn = -(1-fromCAMvaluesThreshold1[]/150);
turn = 0.5;
} else if (fromCAMvaluesThreshold1[1]>90){
turn = -0.5;
} else {
turn = 0;
blobtimer++;
}
}
if (blobtimer > 1000){
state_1 = state;
state = 20;
}
break;
case 20: //shooting code
displayLEDletter(2);
Vref = 0;
turn = 0;
if (state != state_1) {
shoottimer = 0;
state_1 = state;
//GpioDataRegs.GPASET.bit.GPIO8 = 1;
} else {
shoottimer++;
}
if (shoottimer == 500){ // Waits two seconds
//GpioDataRegs.GPACLEAR.bit.GPIO8 = 1;
state_1 = state;
state = 30;
}
break;
case 30: // Drive to look for April tag (-7,0)
displayLEDletter(3);
x_ref = -7;
y_ref = 0;
if (state_1 != state){
attarget = 0;
state_1 = state;
}
if (attarget == 1){
Vref = 0;
turn = 0;
state_1 = state;
state = 31;
}
break;
case 31:
displayLEDletter(3);
if (state_1 != state){
if (pose > PI) {
thetarel = pose - 2.0*PI*floorf((pose+PI)/(2.0*PI));
} else if (pose < -PI) {
thetarel = pose - 2.0*PI*ceilf((pose-PI)/(2.0*PI));
} else {
thetarel = pose;
}
angleCounter = pose - thetarel;
state_1 = state;
}
if (pose > angleCounter + 0.2){
turn = -0.5;
} else if (pose < angleCounter - 0.2) {
turn = 0.5;
} else {
state_1 = state;
state = 32;
}
break;
case 32: // Turn until sees April tag--when sees april tag, turn to centered, if centered drive straight
displayLEDletter(3);
Vref = 0;
turn = 0;
if (state_1 != state){
apriltimer = 0;
state_1 = state;
}
apriltimer++;
if (apriltimer > 1000){
if (fromCAMvaluesThreshold2[1] == 0){
turn = -0.25;
}
}
if (fromCAMvaluesThreshold2[1] != 0){
if (fromCAMvaluesThreshold2[1]<70){ // Center the bot on tag
// turn = -(1-fromCAMvaluesThreshold1[]/150); // Idea for controlled speed turns
turn = 0.25;
} else if (fromCAMvaluesThreshold2[1]>90){
turn = -0.25;
} else {
turn = 0;
Vref = 0.5; // Drive toward if centered
}
if ((fromCAMvaluesThreshold2[0] > -3.0) && (fromCAMvaluesThreshold2[0] < 0.0)){
if (locked == 0){
lockedtimer = 0;
} else {
lockedtimer++;
}
locked = 1;
Vref = 0;
if (lockedtimer > 250){
state_1 = state;
state = 40;
}
}
} else {
locked = 0; // Locked onto target always set to false if tag not seen
}
break;
case 40: // wait for refill
Vref = 0;
turn = 0;
displayLEDletter(4);
Vref = 0;
turn = 0;
if (state_1 != state){
waittime = 0;
state_1 = state;
}
else {
waittime++;
}
if (waittime = 1000) { //wait 4 seconds
state_1 = state;
state = 404;
}
break;
default:
displayLEDletter(8);
serial_printf(&SerialA, "State error: %d \r\n", state); // Debug wrong state
break;
}
}
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ gpio pin to tell second board to shoot
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(8, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO8 = 0;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Blue LED on LaunchPad
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);
// 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.SCIB_RX_INT = &RXBINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
//____________________________________________________________________________________________________________________________
//Lab 5 work
PieVectTable.SPIB_RX_INT = &SPIB_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 given period:
// 200MHz CPU Freq, Period (in uSeconds)
//____________________________________________________________________________________________________________________________
ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 1000);
ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 4000);
ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 20000);
//____________________________________________________________________________________________________________________________
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serialSCIA(&SerialA,115200);
init_serialSCIB(&SerialB,115200);
// init_serialSCIC(&SerialC,115200);
// init_serialSCID(&SerialD,115200);
//____________________________________________________________________________________________________________________________
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); // Set up GPIO2/3 as EPWM2 pins
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);
setupSpib(); // Lab 5: Call the SPIB setup
init_eQEPs(); // Lab 6: Call the encoder setup
init_EPWMs(); // Lab 6: Call the ePWM setup
IER |= M_INT6; // Enable interrupt group 6
//____________________________________________________________________________________________________________________________
// Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
// which is connected to CPU-Timer 1, and CPU int 14, which is connected
// to CPU-Timer 2: int 12 is for the SWI.
IER |= M_INT1;
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
//____________________________________________________________________________________________________________________________
// Lab 5 work
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
//____________________________________________________________________________________________________________________________
// 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, "CAMERA VALUE1: %.4f , CAMERA VALUE2: %.4f \r\n", fromCAMvaluesThreshold1[0], fromCAMvaluesThreshold1[1]);
// Lab 5 work
// serial_printf(&SerialA, "id:%d count:%d busy:%d angleCounter: %.4f pose: %.4f width:%.4f \r\n", identifier, counter, busy, angleCounter, pose, width); // FOR TESTING width
//____________________________________________________________________________________________________________________________
//serial_printf(&SerialA,"Num Timer2:%ld Num SerialRX: %ld\r\n",CpuTimer2.InterruptCount,numRXA);
UARTPrint = 0;
}
}
}
//_______________________________________________________________________________________________________________________________
// Lab 6: Excercise 1
// Void function setting up pins for reading quadrature encoder
void init_eQEPs(void) {
// setup eQEP1 pins for input
EALLOW;
//Disable internal pull-up for the selected output pins for reduced power consumption
GpioCtrlRegs.GPAPUD.bit.GPIO20 = 1; // Disable pull-up on GPIO20 (EQEP1A)
GpioCtrlRegs.GPAPUD.bit.GPIO21 = 1; // Disable pull-up on GPIO21 (EQEP1B)
GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2; // Qual every 6 samples
GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; // Qual every 6 samples
EDIS;
// This specifies which of the possible GPIO pins will be EQEP1 functional pins.
// Comment out other unwanted lines.
GPIO_SetupPinMux(20, GPIO_MUX_CPU1, 1); // Pins to EQEP1A/B
...
This file has been truncated, please download it to see its full contents.
Comments
Please log in or sign up to comment.