Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
 |
| × | 1 | |||
 |
| × | 1 | |||
Software apps and online services | ||||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
The project consisted of a previously constructed Self Balancing Robot (Segbot) that has been constructed over the past few months and an additional ultrasonic sensor. The ultrasonic's sensors purpose is to measure the distance an object is from the sensor. The main goal of the project was to use the ultrasonic sensor and 24 LED's in order to display the distance value to the user. The first step was to figure out how exactly the sensor worked. There are two essential pins on the ultrasonic sensor: the trigger pin and the echo pin. The trigger pin acts as the input signal by sending out an ultrasonic wave. When the wave is sent out the trigger pin is pulled from high to low, meanwhile the echo pin is pulled from low to high. When the echo pin receives the wave, the pin will then be pulled from high to low. The C program is designed to measure how long the echo pin is set to high, which is essentially the time is takes for the signal to leave the sensor, travel to the object, and travel back to the sensor. Taking this value then multiplying it by the speed of sound and dividing it by 2 will provide you with the total distance. The next step was to look at how the sensor is is setup with the Texas Instruments F28379D Launchpad board. As mentioned there is a trigger pin and an echo pin, but there is also a VCC pin and ground pin.
Ultrasonic Sensor Pins
The GPIO's on the PCB board are connected to the Launchpad pins, which allowed for an easy connection. The trigger pin was set to GPIO0 which is Launchpad pin 40, the echo pin was set to GPIO19 which is Launchpad pin 3. The VCC pin was connected to 5V, and then grounded. *The circuit schematics are included in the attachments tab.
Launchpad Wiring for Ultrasonic Sensor
Once this value was obtained, the main focus was writing a C program using Code Composer Studio in order to manipulate the distance value. The maximum distance of the sensor is around 300 cm, so the plan of action was to parse the echo distance value into 3 separate digits and put those into an array for ease of computation later in the program. This was done with the following code:
Parsing the Distance Value
Once this was done, it was time to define the arrays. There was an issue that I kept running into with the code. The fact that the numbers from 0 - 99 were followed by zeros when outputting to the LEDs (Ex: 006 cm), and that if the value was 118 cm you wouldn't be able to tell the difference between 18 cm because the repeating digit. The following if statements fix this issue where the "13" acts as a break in the code.
Defining the Arrays
The array was then looped through based off the CPU interrupt timer on the Launchpad, with the value then passed to a new function which would output the digits on the LEDs shown below.
24 LEDs to Output Distance Reading
With the use of switch cases in the program, it was quite simple to output the values to the LED's by setting the correct GPIO pins to high for the correct formation of the number. For example, the following is what would occur if the digit the loop was on was 0 or 1:
Code to Turn on the LEDs
Once the code was integrated with the balancing code, it was ready to construct a casing for the ultrasonic sensor. One thing that I noticed was that when the ultrasonic sensor is not high enough from the robot, the values become quite distorted. This fueled the inspiration for the holder made out of cardboard that was placed on top of the robot.
Cardboard Holder for Ultrasonic Sensor
The design was finally complete and running smoothly. A demo video is shown below with the final result. The measurements were triggered when the 'm' key was pressed on the keyboard, with the steering controlled by 'w', 'a', 's', 'd.'
Thank you for reading and if you have any questions with the project please feel free to contact Dan Block at d-block@illinois.edu.
//#############################################################################
// FILE: finalproject_main_balancingrobotversion.c
//
// TITLE: FINAL PROJECT FOR BALANCING ROBOT
//#############################################################################
// 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"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
// 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);
__interrupt void SPIB_isr(void);
void serialRXA(serial_t *s, char data);
void setEPWM6A(float controleffort);
void setEPWM6B(float controleffort);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setupSpib(void);
/*--------------COUNT VARIABLES-------------------*/
uint32_t numTimer0calls = 0;
uint32_t numTimer1calls = 0;
uint32_t numTimer2calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numTimerADCAcalls = 0;
uint32_t numRXA = 0;
uint32_t numSPIBcalls = 0;
uint16_t UARTPrint = 0;
int16_t spivalue1 = 0;
int16_t spivalue2 = 0;
/*--------------MPU VARIABLES-------------------*/
int16_t dummy =0;
int16_t accelXraw = 0.0;
int16_t accelYraw = 0.0;
int16_t accelZraw = 0.0;
float accelx = 0.0;
float accely = 0.0;
float accelz = 0.0;
int16_t temp_raw = 0.0;
float temp_reading = 0.0;
int16_t gyroXraw = 0.0;
int16_t gyroYraw = 0.0;
int16_t gyroZraw = 0.0;
float gyrox = 0.0;
float gyroy = 0.0;
float gyroz = 0.0;
/*--------------POSITION AND VELOCITY VARIABLES-------------------*/
float radLeftWheel=0;
float radRightWheel=0;
float distLeftWheel = 0;
float distRightWheel = 0;
float uLeft = 5.0;
float uRight = 5.0;
float vRight = 0;
float vLeft = 0;
float prev_distLeftWheel = 0;
float prev_distRightWheel = 0;
float err_Left = 0;
float err_Right = 0;
float prev_err_Right = 0;
float prev_err_Left = 0;
float ILeft = 0;
float IRight = 0;
float prev_ILeft = 0;
float prev_IRight = 0;
float Vref = 0.1; //reference velocity
float Kp_turn = 3.0;
float err_turn = 0;
float turn = 0;
//ADCA Variables
float ADC1Volts = 0;
float ADC2Volts = 0;
int16_t scaledADCINA2;
int16_t scaledADCINA3;
float fscaledADCINA2=0;
float fscaledADCINA3=0;
/*--------------BALANCE VARIABLES-------------------*/
float LeftWheel = 0.0;
float RightWheel = 0.0;
float PrevRightWheelRad = 0.0;
float PrevLeftWheelRad = 0.0;
float Vright = 0.0;
float Vleft = 0.0;
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
float gyrox_offset = 0;
float gyroy_offset = 0;
float gyroz_offset = 0;
float accelzBalancePoint = -.76;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
float vel_left = 0;
float vel_left_1 = 0;
float vel_right = 0;
float vel_right_1 = 0;
float LeftWheel_1 = 0;
float RightWheel_1 = 0;
float ubal = 0;
float K1 = -30;
float K2 = -2.8;
float K3 = -1.0;
float Kp = 3.0;
float Ki = 20.0;
float Kd = 0.08;
/*--------------KALMAN FILTER VARIABLES-------------------*/
float T = 0.001; //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
/*--------------STEERING VARIABLES-------------------*/
float WhlDiff = 0;
float WhlDiff_1 = 0;
float vel_WhlDiff = 0;
float vel_WhlDiff_1 = 0;
float turnref = 0;
float errorDiff = 0;
float errorDiff_1 = 0;
float intDiff = 0;
float intDiff_1 = 0;
float FwdBckOffset = 0;
float turnrate = 0;
float turnrate_1;
float turnref_1;
//9.185 rads per foot
/*--------------JOYSTICK VARIABLES-------------------*/
//Filter
#define FilterOrder 10
//float xk[FilterOrder];
float yk = 0;
float xk[FilterOrder];
float b[FilterOrder]={1.1982297073578186e-02,
3.2593697188218529e-02,
8.8809724362308426e-02,
1.5903360855022139e-01,
2.0758067282567344e-01,
2.0758067282567344e-01,
1.5903360855022139e-01,
8.8809724362308426e-02,
3.2593697188218529e-02,
1.1982297073578186e-02};
/*--------------ALL THE ULTRASONIC SENSOR DEFINITIONS/VARIABLES-------------------*/
#define TIMEBASE 0.005 // 1.0/200
// VCC - +5V
// GND - GND
// TRIG - GPIO0
// ECHO - GPIO19
//this is the ecap1 interrupt
__interrupt void ecap1_isr(void);
void setup_led_GPIO(void);
void OutputLED(int digit_to_LED);
int32_t echo_count = 0;
float echo_duration = 0;
float echo_distance = 0;
uint16_t trigger_count = 0;
uint16_t trigger_state = 0;
int ecap1count = 0;
int rounded_distance = 0;
int first_digit = 0;
int second_digit = 0;
int third_digit = 0;
int echo_distance_1 = 0;
int arraycount = 0;
int digit_to_LED = 0;
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
trigger_state = 2;
}
trigger_count++;
} else {
if (trigger_state == 2) {
trigger_count = 0;
trigger_state = 0;
}
}
}
// InitECapture - Initialize ECAP1 configurations
//Enhanced Capture
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
}
void main(void){
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
setup_led_GPIO();
//SETUP PIN MUX FOR RIGHT MOTOR
GPIO_SetupPinMux(10, GPIO_MUX_CPU1, 1);
//SETUP PIN MUX FOR LEFT MOTOR
GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 1);
//EXERCISE 1: SETUP PIN MUX FOR EPWM5A/B
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 1);
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 1);
// Trigger pin for HC-SR04
//pull low
GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO0 = 1;
// Echo pin for HC-SR04
//choose GPIO 19, can work for any gpio
EALLOW;
InputXbarRegs.INPUT7SELECT = 19; // Set eCAP1 source to GPIO-pin
EDIS;
GPIO_SetupPinOptions(19, GPIO_INPUT, GPIO_ASYNC);
// 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(52, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
// LED3
GPIO_SetupPinMux(67, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(67, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO67 = 1;
// LED4
GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
// LED5
GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
// LED6
GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
// LED7
GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
// LED8
GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;
// LED9
GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;
// LED10
GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(4, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO4 = 1;
// LED11
GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(5, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO5 = 1;
// LED12
GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(6, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO6 = 1;
// LED13
GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(7, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO7 = 1;
// LED14
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(8, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO8 = 1;
// LED15
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;
// LED16
GPIO_SetupPinMux(24, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(24, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO24 = 1;
// LED17
GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;
// LED18
GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;
// LED19
GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;
// LED20
GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
// LED21
GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
// LED22
GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;
// LED23
GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO158 = 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;
//MPU9250 CS Chip Select
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
//PushButton 1
GPIO_SetupPinMux(122, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(122, GPIO_INPUT, GPIO_PULLUP);
//PushButton 2
GPIO_SetupPinMux(123, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(123, GPIO_INPUT, GPIO_PULLUP);
//PushButton 3
GPIO_SetupPinMux(124, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(124, GPIO_INPUT, GPIO_PULLUP);
//PushButton 4
GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(125, GPIO_INPUT, GPIO_PULLUP);
EALLOW;
//EXERCISE 1: EPWM5 Registers
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; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
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 = 50000; // .001/(1/50000000) Set Period to 1ms sample. Input clock is 50MHz.
//EPwm5Regs.TBPRD = 2000; // 25kHZ Set period to .25 ms sample
// 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;
//EPWM6A/B REGISTERS
EPwm6Regs.TBCTL.bit.CTRMODE = 0; //Count Up Mode
EPwm6Regs.TBCTL.bit.FREE_SOFT = 3; //Free soft emulation mode to Free Run, PWM continues when you set a BP in your code
EPwm6Regs.TBCTL.bit.PHSEN = 0; //Disabled phase loading
EPwm6Regs.TBCTL.bit.CLKDIV = 0; //Clock divide by 1
EPwm6Regs.TBCTR = 0; //Start timers at 0
EPwm6Regs.TBPRD = 2500;
EPwm6Regs.CMPA.bit.CMPA = 0;
EPwm6Regs.CMPB.bit.CMPB = 0;
EPwm6Regs.AQCTLA.bit.ZRO = 2; //Set when compareA is reached
EPwm6Regs.AQCTLA.bit.CAU = 1; //Clear when compareA is reached
EPwm6Regs.AQCTLB.bit.ZRO = 2; //Set when compareA is reached
EPwm6Regs.AQCTLB.bit.CBU = 1; //Clear when compareA is reached
//EPwm6RegsTBPHS.bit.TBPHS = 0; //Set phase to zero
EDIS;
// Enable DACA and DACB outputs
EALLOW;
//write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcdRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
//Set pulse positions to late
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
//power up the ADCs
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//ADCA
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 0x2; //SOC0 will convert Channel you choose Does not have to be A0
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 13;//NOT SURE EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 0x3; //SOC1 will convert Channel you choose Does not have to be A1
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
EALLOW;
DacaRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacA output-->uses ADCINA0
DacaRegs.DACCTL.bit.LOADMODE = 0; //load on next sysclk
DacaRegs.DACCTL.bit.DACREFSEL = 1; //use ADC VREF as reference voltage
EDIS;
InitECapture();
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
setupSpib();
init_eQEPs(); // EX1: calling this function
// 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.EMIF_ERROR_INT = &SWI_isr;
//ADCA enabled
PieVectTable.ADCA1_INT = &ADCA_ISR;
//SPIB enabled
PieVectTable.SPIB_RX_INT = &SPIB_isr;
PieVectTable.ECAP1_INT = &ecap1_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, 4000); //EX1: timer interrupts every 4 millisecond
ConfigCpuTimer(&CpuTimer2, 200, 500000);
// 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);
// 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; //ADCA1
IER |= M_INT2;
IER |= M_INT4; // Enable CPU INT4 which is connected to ECAP1-4 INT
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
// SPIB
IER |= M_INT6;
// Enable eCAP INT1 in the PIE: Group 4 interrupt 1
PieCtrlRegs.PIEIER4.bit.INTx1 = 1;
// Enable for SPIB
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
// 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 ACDA1 in the PIE: Group 1 interrupt 1
PieCtrlRegs.PIEIER1.bit.INTx1 = 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,"Tilt Value = %.3f Gyro Value = %.3f Left Wheel = %.3 Right Wheel = %.3f \n\r", tilt_value, gyro_value, LeftWheel,RightWheel);
//serial_printf(&SerialA,"V1 = %.3f V2 = %.3f GX = %.3f AZ = %.3f L = %.3f R = %.3f \n\r", fscaledADCINA2, fscaledADCINA3, gyrox, accelz, LeftWheel, RightWheel);
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
/*-----------BALANCE AND STEERING CODE-------------*/
//solve diff eqns for vel_Right and vel_Left
vel_left = 0.6*vel_left + 100*(LeftWheel - LeftWheel_1);
vel_right = 0.6*vel_right + 100*(RightWheel - RightWheel_1);
//define WhlDiff
WhlDiff = LeftWheel - RightWheel; //difference in radians between wheels
//solve diff eqns for vel_WhlDiff
vel_WhlDiff = 0.333*vel_WhlDiff_1 + 166.667*(WhlDiff - WhlDiff_1); //New Diff Eqn
//integrate turnrate using trapezoid rule to find turnref
turnref = turnref_1 + ((turnrate + turnrate_1)/2)*0.004;
//balance equation
ubal = -K1*tilt_value - K2*gyro_value - K3*((vel_left + vel_right)/2.0);
//find error between turnref and feedback signal WhlDiff
errorDiff = turnref - WhlDiff;
//integrate error diff using trapezoid rule
intDiff = intDiff_1 + ((errorDiff + errorDiff_1)/2)*0.004;
//PID K values
Kp = 3.0;
Ki = 20.0;
Kd = 0.08;
//PID Control
turn = Kp*errorDiff + Ki*intDiff - Kd*vel_WhlDiff;
//getting rid of the burn-off
if (fabs(turn) >= 3){
intDiff = intDiff_1;
}
//saturate turn b/w -4 and 4
if (turn > 4) {
turn = 4;
}
if (turn < -4) {
turn = -4;
}
//calculate controll effor for left and right wheel
uRight = ubal/2.0 - turn + FwdBckOffset;
uLeft = ubal/2.0 + turn + FwdBckOffset;
//send values to setEPWMA/B functions
setEPWM6A(uLeft);
setEPWM6B(-uRight);
//save past states
vel_left_1 = vel_left;
LeftWheel_1 = LeftWheel;
vel_right_1 = vel_right;
RightWheel_1 = RightWheel;
WhlDiff_1 = WhlDiff;
vel_WhlDiff_1 = vel_WhlDiff;
intDiff_1 = intDiff;
errorDiff_1 = errorDiff;
turnrate_1 = turnrate;
turnref_1 = turnref;
numSWIcalls++;
DINT;
}
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
numTimer0calls++;
if ((numTimer0calls%50) == 0) {
PieCtrlRegs.PIEIFR12.bit.INTx9 = 1; // Manually cause the interrupt for the SWI
}
hc_sr04_trigger();
// Blink LaunchPad Red LED
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
__interrupt void cpu_timer1_isr(void)
{
CpuTimer1.InterruptCount++;
numTimer1calls++;
//
// if ((numTimer1calls%100) == 0) {
// UARTPrint = 1;
// }
}
__interrupt void cpu_timer2_isr(void)
{
// Blink LaunchPad Blue LED
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
CpuTimer2.InterruptCount++;
// if ((CpuTimer2.InterruptCount % 50) == 0) {
// UARTPrint = 1;
// }
numTimer2calls++;
int digit_array[8];
//Note: 13 is used here as a break. it displays nothing on the LEDs
//for numbers 0 - 9
if ((first_digit == 0) && (second_digit == 0)){
digit_array[0] = 13;
digit_array[1] = 13;
digit_array[2] = 13;
digit_array[3] = 13;
digit_array[4] = third_digit;
digit_array[5] = 11;
digit_array[6] = 12;
digit_array[7] = 13;
}
// for numbers 10 - 99
else if (first_digit == 0){
digit_array[0] = 13;
digit_array[1] = 13;
digit_array[2] = second_digit;
digit_array[3] = 13;
digit_array[4] = third_digit;
digit_array[5] = 11;
digit_array[6] = 12;
digit_array[7] = 13;
}
//for numbers 100 - 999
else if (first_digit > 0){
digit_array[0] = first_digit;
digit_array[1] = 13;
digit_array[2] = second_digit;
digit_array[3] = 13;
digit_array[4] = third_digit;
digit_array[5] = 11;
digit_array[6] = 12;
digit_array[7] = 13;
}
else if ((first_digit > 0) && (second_digit > 0)){
digit_array[0] = first_digit;
digit_array[1] = 13;
digit_array[2] = second_digit;
digit_array[3] = 13;
digit_array[4] = third_digit;
digit_array[5] = 11;
digit_array[6] = 12;
digit_array[7] = 13;
}
//this loops through the array on the interrupt timer
arraycount++;
if (arraycount > 7){
arraycount = 0;
}
digit_to_LED = digit_array[arraycount]; //set array value to new int variable
OutputLED(digit_to_LED); //send int variable to OutputLED function
}
//updates our echo distance
__interrupt void ecap1_isr(void) {
//only need to measure how long echo is high
PieCtrlRegs.PIEACK.all = PIEACK_GROUP4; // Must acknowledge the PIE group
ECap1Regs.ECCLR.bit.INT = 1; // Clear the ECAP1 interrupt flag
ECap1Regs.ECCLR.bit.CEVT3 = 1; // Clear the CEVT3 flag
// The eCAP is running at the full 200 MHz of the device.
//1.0/200MHz = 0.005 TIMEBASE
// Captured values reflect this time base.
// Compute the PWM duty period (rising edge to falling edge)
echo_count = (int32)ECap1Regs.CAP2 - (int32)ECap1Regs.CAP1; //how many counts for the echo pin to stay high
//timebase is limiting factor of trigger
echo_duration = (float)(TIMEBASE * echo_count); // in us //is how long echo pin stays high in micro
echo_distance = echo_duration * 0.034 / 2.0; // cm //0.034 = speed of sound //use watch expression window
rounded_distance = round(echo_distance_1);
first_digit = rounded_distance % 1000 / 100; //hundreds place
second_digit = rounded_distance % 100 / 10; //tens place
third_digit = rounded_distance % 10; //ones place
ecap1count++;
}
__interrupt void ADCA_ISR (void)
{
/*------------Joystick Code--------------------*/
numTimerADCAcalls++;
scaledADCINA2 = AdcaResultRegs.ADCRESULT0;
scaledADCINA3 = AdcaResultRegs.ADCRESULT1;
// Here convert ADCIND0, ADCIND1 to volts
fscaledADCINA2 = (scaledADCINA2/4095.0)*3.0;
fscaledADCINA3 = (scaledADCINA3/4095.0)*3.0;
xk[0] = fscaledADCINA2;
int i;
yk = 0;
for(i=0; i < FilterOrder; i++) {
yk = yk +(b[i]*xk[i]);
}
for (i=1; i < FilterOrder; i++) {
xk[FilterOrder - i] = xk[FilterOrder - 1 - i];
}
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 8; //how many things we want to read
SpibRegs.SPITXBUF = ((0x8000) | (0x3A00)); //hex 8 means start reading, 3A is where we want to read address
SpibRegs.SPITXBUF = 0; //read the next 7 values in the register
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
//Print ADCIND0 and ADCIND1’s voltage value to Terminal every 100ms
if ((numTimerADCAcalls % 200) == 0)
UARTPrint = 1;
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
...
This file has been truncated, please download it to see its full contents.
Comments