Hardware components | ||||||
 |
| × | 1 | |||
| × | 1 | ||||
 |
| × | 1 | |||
Software apps and online services | ||||||
 |
| |||||
 |
| |||||
 |
| |||||
This project is 80-90% complete and needs further improvement. In this project we designed a robot that can solve mazes by first traversing either manually or on autopilot. The robot will understand where it has been via waypoints that are either added manually or during turns. Redundant waypoints are removed by looking through a radius around waypoints added two turns ago or, generally, waypoints two pointers lower on the stack. That is, the robot only removes redundant waypoints in consideration of the case of dead-ends.
The parts that are completed are:- Full controller support for manual control of the robot (includes manual override)
- Autopilot capability via wall-following (toggleable)
- Voice command
- Waypoint generation
- Backwards traversal
Full controller support for the intended solution - Joystick controls turn commands, triggers control forwards and backwards velocity, and the buttons each control different tasks: A - Add Waypoint, B - Backwards Traversal, X - Killswitch, Y - Autopilot/Wall-Following. (Manual override refers to the nature of the velocity command to return control of the robot to the controller when changed manually).
Figure 1. Robot with Xbox Controller
Autopilot uses a wall-following algorithm based on one from UIUC ME 461 Lab 6. (Toggleable refers to how the Y button will toggle the autopilot on - this algorithm can be manually overridden by the controller to return back to manual control).
Voice command works via microphone with multiple bandpass filters to allow for wireless control of the robot given an audio frequency generator as an alternative control method. In our project, the frequency signal was converted to voltage signal ranging from approximately 0.5V to 2.5V by an analog to digital converter (ADC), which is further used as control value. Also, a 31st-order two-channel bandpass FIR was applied to detect the 500Hz and 2000Hz noise. If a 500Hz noise is generated, the robot will start moving and perform specific actions under the command of Xbox controller. Whereas if a 2000Hz noise is generated, the robot would stop moving and act as a killswitch.
Waypoint generation functions with manual addition of waypoints and currently with a polling method of generating new waypoints. The waypoint stack was pessimistically sized for 100 waypoints, however, this can be expanded given memory permits. To be completed is the testing for the waypoint generator that detects when turns are made. The goal of the turn generating waypoints is to allow for pure pursuit of waypoints for the backwards traversal.
Backwards traversal functions on the concept of gradient descent and dead reckoning. The robot engages in pure pursuit of 1 waypoint and uses error vectors to correct both heading and position. Overshoot of waypoints has not entirely been handled by this case as it was found to reliably reach waypoints head-on. The current bug with this function is a seeming rotation ranging between 30-90 degrees each time the backwards traversal is initiated. That is, the robot reference frame seems to have a disconnect with the waypoint reference frame, but experimenting with frame rotations was inconclusive. A proposed duct-tape solution is to fix the rotation case by manually setting the heading on the robot to align with the axes of the waypoint coordinate space. This proposed solution could either be hardcoded or performed manually by winding up the heading via the robot's wheels.
Video:
main.c
C/C++This code contains most of the algorithms for wall following, controller functionality, frequency filters, PWM and ADC initialization. We used lab 6: dc motor speed control as a starting point for our final project.
//#############################################################################
// FILE: LAB6starter_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"
#include "user_xy.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
// ----- code for CAN start here -----
#include "F28379dCAN.h"
//#define TX_MSG_DATA_LENGTH 4
//#define TX_MSG_OBJ_ID 0 //transmit
#define RX_MSG_DATA_LENGTH 8
#define RX_MSG_OBJ_ID_1 1 //measurement from sensor 1
#define RX_MSG_OBJ_ID_2 2 //measurement from sensor 2
#define RX_MSG_OBJ_ID_3 3 //quality from sensor 1
#define RX_MSG_OBJ_ID_4 4 //quality from sensor 2
// ----- code for CAN end here -----
// 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);
// ----- code for CAN start here -----
__interrupt void can_isr(void);
// ----- code for CAN end here -----
__interrupt void SPIB_isr(void);
__interrupt void ADCB_ISR(void);
void setupSpib(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
float sat(float input, float satval);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
void setEPWM2(float controleffort, bool ab);
void setDACA(float dacouta0);
void setDACB(float dacouta1);
static inline float velocity(float* th_old,float* omega_old1,float* omega_old2,float th_motor);
static inline float integral(float integ,float e1,float* e2);
// static inline float heading(float des_x,float des_y,float x,float y,float ref_thp);
static inline void waypoint_gen(float xp,float yp,int* stck_ptr,float* xstck,float* ystck);
static inline void flag_check(float turn,float turn_thresh,bool* pt_flag,bool* pt_lock);
static inline void flag_set(bool* pt_flag,bool* pt_lock);
static inline int autopylot(bool autopilot,int ctrl);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numTimer1calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
int16_t accelx_raw = 0;
int16_t accely_raw = 0;
int16_t accelz_raw = 0;
int16_t gyrox_raw = 0;
int16_t gyroy_raw = 0;
int16_t gyroz_raw = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
//raaddfdfiians
float radl = 0;
float radr = 0;
bool hard_reset = 1;
float radius = 0.061;
float distance = 0;
//CONTROL VARIABLES (I'M LOSING CONTROL I'M LOSING CONTROL I'M LOSING CONTRO...)
float vref = 0.25; // m/s
float Kp = 3.0;
float Ki = 25.0;
float uLeft = 0.0;
float uRight = 0.0;
float thresh = 10.0;
float turn = 0.0;
float eturnal = 0.0;
float Kp_t = 3.0;
//Final Lab Temp Control Variables
float vref_temp = 0;
float turn_temp = 0;
float des_x_temp = 0;
float des_y_temp = 0;
bool labview_ctrl = 0;
//velocity variables
float omegal = 0;
float old_omegal1 = 0;
float old_thl = 0;
float omegar = 0;
float old_omegar1 = 0;
float old_thr = 0;
float vl = 0;
float vr = 0;
//integral variables
float left_e1 = 0;
float right_e1 = 0;
float left_e2 = 0;
float right_e2 = 0;
float I1 = 0;
float I2 = 0;
float I1_old = 0, I2_old = 0;
// exercise 5 labview variables
float printLV3 = 0;
float printLV4 = 0;
float printLV5 = 0;
float printLV6 = 0;
float printLV7 = 0;
float printLV8 = 0;
float x = 0;
float y = 0;
float bearing = 0;
extern uint16_t NewLVData;
extern float fromLVvalues[LVNUM_TOFROM_FLOATS];
extern LVSendFloats_t DataToLabView;
extern char LVsenddata[LVNUM_TOFROM_FLOATS*4+2];
extern uint16_t newLinuxCommands;
extern float LinuxCommands[CMDNUM_FROM_FLOATS];
int32_t SpibNumCalls = 0;
// Robot Pose Variables
float Rw = 0.0593; //Wheel Radius
float Wr = 0.173; //Robot Width (Wheel to Wheel distance)
float xr = 0.0; //Robot Pose X Coordinate
float xdr = 0.0; //Robot Pose X Coordinate Dot
float yr = 0.0; //Robot Pose Y Coordinate
float ydr = 0.0; //Robot Pose X Coordinate Dot
float thl = 0.0; //Left Wheel Rotation Angle (theta)
float thdl = 0.0; //Left Wheel Angular Velocity (theta dot)
float thr = 0.0; //Right Wheel Rotation Angle (theta)
float thdr = 0.0; //Right Wheel Angular Velocity (theta dot)
float thp = 0.0; //Robot Pose Angle/Bearing (theta)
float thavg = 0.0; //Average Rotation Angle (theta)
float thdavg = 0.0; //Average Angular Velocity (theta dot)
float xr_old = 0.0;
float xdr_old = 0.0;
float yr_old = 0.0;
float ydr_old = 0.0;
//Exercise 7 Extra Variables
float distr = 0.0; //distright in the lab doc
float distf = 0.0; //distfront in the lab doc
float ref_right = 300;
float ref_left = 300;
float ref_front = 1400;
//7 control
float kpr = 0.0003; //kp,right in the lab doc
float kpf = 0.0005; //kp,front in the lab doc
int right_wall_follow = 0;
//exercise 8
bool Dtrue = 1;
bool tLtrue = 1;
// Lab Final variables ------------------------------------------------------------
bool killswtch = 0;
bool killtemp = 0;
bool man_gen = 0; //for setting waypoints manually
bool a_temp = 0;
bool ctrl_temp = 0;
bool autopilot = 0;
bool prev_ctrl = 0;
// int autopilot_time =0; //for adding a delay to changes in autopilot
float dead_radius = 2.0; //radius for filtering out dead-ends
int left_wall_follow = 0;
// bool boomerang = 0;
int arrived = 0;
float distl = 0.0;
// float des_x = 0.0;
// float des_y = 0.0;
int ctrl = 1; //controller mux control signal
//int ctrl = 2; //testing XY controller
//0 == wall_follow
//1 == remote control
//2 == boomerang mode
//waypoint stack
float xstck[100];
float ystck[100];
int stck_ptr = 0;
//int stck_ptr = 5;
float turn_thresh = 30.0; //decides if we made a maze turn or not (will need to be tuned)
bool pt_flag = 0; //flag to determine we save a waypoint
bool pt_flag_old = 0;
bool pt_flag_old2 = 0;
bool pt_lock = 0;
// int turn_timer = 0;
float way_send = 0; //boolean to determine if the waypoint should be recorded in Labview or not
float init_head = 0;
float des_head = 0; //desired heading position
bool init = 1;
//xy controller testing
// float test_xstck[6] = {-0.9,0,-0.6,0,-0.3,0};
// float test_ystck[6] = {2.4,2,1.6,1.2,0.8,0.4};
float pt_scale = 1;
float test_xstck[6] = {-3,0,-2,0,-1,0.1};
float test_ystck[6] = {6,5,4,3,2,1};
float alpha = 0;
float theta = 0;
float turnerror = 0;
//initial desireds for testing
float des_x = 0; //0
float des_y = 0; //1
// float turnoff = 0.3;
// ----- code for CAN start here -----
// volatile uint32_t txMsgCount = 0;
// extern uint16_t txMsgData[4];
volatile uint32_t rxMsgCount_1 = 0;
volatile uint32_t rxMsgCount_3 = 0;
extern uint16_t rxMsgData[8];
uint32_t dis_raw_1[2];
uint32_t dis_raw_3[2];
uint32_t dis_1 = 0;
uint32_t dis_3 = 0;
uint32_t quality_raw_1[4];
uint32_t quality_raw_3[4];
float quality_1 = 0.0;
float quality_3 = 0.0;
uint32_t lightlevel_raw_1[4];
uint32_t lightlevel_raw_3[4];
float lightlevel_1 = 0.0;
float lightlevel_3 = 0.0;
uint32_t measure_status_1 = 0;
uint32_t measure_status_3 = 0;
volatile uint32_t errorFlag = 0;
// voice control variables
uint16_t adcd0result = 0;
uint16_t adcd1result = 0;
uint16_t adca0result = 0;
uint16_t adca1result = 0;
uint16_t adcb0result = 0;
uint16_t order = 31;
float voltage_d0 = 0;
float voltage_d1 = 0;
float voltage_output = 0;
uint32_t PRINTsignal = 0;
float xkb[32] = {0};
float ykb = 0;
float b[32] = {0.00405925116488510, 0.00505236048009726, -0.00278473371406261, -0.0135880761500635,
-0.00621797777418964, 0.0231494096217402, 0.0314790310289965, -0.0156670014213808,
-0.0635311180912143, -0.0237448567887860, 0.0727978922981923, 0.0827494094211454,
-0.0349434745161310, -0.121768969491469, -0.0395101744501735, 0.105975861048285,
0.105975861048285, -0.0395101744501735, -0.121768969491469, -0.0349434745161310,
0.0827494094211454, 0.0727978922981923, -0.0237448567887860, -0.0635311180912143,
-0.0156670014213808, 0.0314790310289965, 0.0231494096217402, -0.00621797777418964,
-0.0135880761500635, -0.00278473371406261, 0.00505236048009726, 0.00405925116488510};
// ----- code for CAN end here -----
// void SetLEDRowsOnOff(int16_t rows) {
// //row1
// if (rows & 0x0001) {GpioDataRegs.GPASET.bit.GPIO22 = 1;GpioDataRegs.GPESET.bit.GPIO130 = 1;GpioDataRegs.GPBSET.bit.GPIO60 = 1;}
// else {GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;}
// //row2
// if (rows & 0x0002) {GpioDataRegs.GPCSET.bit.GPIO94 = 1;GpioDataRegs.GPESET.bit.GPIO131 = 1;GpioDataRegs.GPBSET.bit.GPIO61 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;}
// //row3
// if (rows & 0x0004) {GpioDataRegs.GPCSET.bit.GPIO95 = 1;GpioDataRegs.GPASET.bit.GPIO25 = 1;GpioDataRegs.GPESET.bit.GPIO157 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;}
// //row4
// if (rows & 0x0008) {GpioDataRegs.GPDSET.bit.GPIO97 = 1;GpioDataRegs.GPASET.bit.GPIO26 = 1;GpioDataRegs.GPESET.bit.GPIO158 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;}
// //row5
// if (rows & 0x0010) {GpioDataRegs.GPDSET.bit.GPIO111 = 1;GpioDataRegs.GPASET.bit.GPIO27 = 1;GpioDataRegs.GPESET.bit.GPIO159 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;}
//
// the following are the functions for turn signals
void left_signal()
{
GpioDataRegs.GPCSET.bit.GPIO95 = 1;
GpioDataRegs.GPESET.bit.GPIO131 = 1;
GpioDataRegs.GPASET.bit.GPIO25 = 1;
GpioDataRegs.GPASET.bit.GPIO26 = 1;
GpioDataRegs.GPBSET.bit.GPIO60 = 1;
GpioDataRegs.GPBSET.bit.GPIO61 = 1;
GpioDataRegs.GPESET.bit.GPIO157 = 1;
GpioDataRegs.GPESET.bit.GPIO158 = 1;
GpioDataRegs.GPESET.bit.GPIO159 = 1;
}
void right_signal()
{
GpioDataRegs.GPASET.bit.GPIO22 = 1;
GpioDataRegs.GPCSET.bit.GPIO94 = 1;
GpioDataRegs.GPCSET.bit.GPIO95 = 1;
GpioDataRegs.GPDSET.bit.GPIO97 = 1;
GpioDataRegs.GPDSET.bit.GPIO111 = 1;
GpioDataRegs.GPESET.bit.GPIO131 = 1;
GpioDataRegs.GPASET.bit.GPIO25 = 1;
GpioDataRegs.GPASET.bit.GPIO26 = 1;
GpioDataRegs.GPESET.bit.GPIO157 = 1;
}
void clear_led()
{
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;
GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;
GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;
GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;
GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;
GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;
GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;
GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;
}
void forward_signal()
{
GpioDataRegs.GPDSET.bit.GPIO97 = 1;
GpioDataRegs.GPESET.bit.GPIO130 = 1;
GpioDataRegs.GPESET.bit.GPIO131 = 1;
GpioDataRegs.GPASET.bit.GPIO25 = 1;
GpioDataRegs.GPASET.bit.GPIO26 = 1;
GpioDataRegs.GPASET.bit.GPIO27 = 1;
GpioDataRegs.GPESET.bit.GPIO158 = 1;
}
void SetLED_D(bool jamessus) {
//row1
// if (jamessus) {GpioDataRegs.GPASET.bit.GPIO22 = 1;GpioDataRegs.GPESET.bit.GPIO130 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;}
// else {GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;}
// //row2
// if (jamessus) {GpioDataRegs.GPCSET.bit.GPIO94 = 1;GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;GpioDataRegs.GPBSET.bit.GPIO61 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;}
// //row3
// if (jamessus) {GpioDataRegs.GPCSET.bit.GPIO95 = 1;GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;GpioDataRegs.GPESET.bit.GPIO157 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;}
// //row4
// if (jamessus) {GpioDataRegs.GPDSET.bit.GPIO97 = 1;GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;GpioDataRegs.GPESET.bit.GPIO158 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;}
// //row5
// if (jamessus) {GpioDataRegs.GPDSET.bit.GPIO111 = 1;GpioDataRegs.GPASET.bit.GPIO27 = 1;GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;}
}
void SetLED_tL(bool jamessus) {
// //row1
// if (jamessus) {GpioDataRegs.GPASET.bit.GPIO22 = 1;GpioDataRegs.GPESET.bit.GPIO130 = 1;GpioDataRegs.GPBSET.bit.GPIO60 = 1;}
// else {GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;}
// //row2
// if (jamessus) {GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;GpioDataRegs.GPESET.bit.GPIO131 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;}
// //row3
// if (jamessus) {GpioDataRegs.GPCSET.bit.GPIO95 = 1;GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;}
// else {GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;}
// //row4
// if (jamessus) {GpioDataRegs.GPDSET.bit.GPIO97 = 1;GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;}
// //row5
// if (jamessus) {GpioDataRegs.GPDSET.bit.GPIO111 = 1;GpioDataRegs.GPASET.bit.GPIO27 = 1;GpioDataRegs.GPESET.bit.GPIO159 = 1;}
// else {GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;}
}
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
xstck[0] = 0; ystck[0] = 0; stck_ptr++; // initialize stacks at (0,0)
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, 0);
GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);
// EXORCISE 2 PIN MUX EPWM2AB
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
GPIO_SetupPinOptions(2, GPIO_INPUT, GPIO_PULLUP);
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);
GPIO_SetupPinOptions(3, GPIO_INPUT, GPIO_PULLUP);
// ----- code for CAN start here -----
//GPIO17 - CANRXB
GPIO_SetupPinMux(17, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(17, GPIO_INPUT, GPIO_ASYNC);
//GPIO12 - CANTXB
GPIO_SetupPinMux(12, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(12, GPIO_OUTPUT, GPIO_PUSHPULL);
// ----- code for CAN end here -----
// ----- code for CAN start here -----
// Initialize the CAN controller
InitCANB();
// Set up the CAN bus bit rate to 1000 kbps
setCANBitRate(200000000, 1000000);
// Enables Interrupt line 0, Error & Status Change interrupts in CAN_CTL register.
CanbRegs.CAN_CTL.bit.IE0= 1;
CanbRegs.CAN_CTL.bit.EIE= 1;
// ----- code for CAN end here -----
// 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.SPIB_RX_INT = &SPIB_isr;
PieVectTable.ADCB1_INT = &ADCB_ISR;
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
// ----- code for CAN start here -----
PieVectTable.CANB0_INT = &can_isr;
// ----- code for CAN end here -----
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, 40000);
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serialSCIA(&SerialA,115200);
EALLOW;
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; // Set Period to 1ms sample. Input clock is 50MHz.
// EPwm5Regs.TBPRD = 12500; // LC and YZ: Set Period to .25ms sample. Input clock is 50MHz.
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;
//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);
//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 = 2; //SOC0 will convert Channel you choose Does not have to be A0
// 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.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel you choose Does not have to be A1
// AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// 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
//ADCB
AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel you choose Does not have to be B0
AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
//AdcbRegs.ADCSOC1CTL.bit.CHSEL = 1; //SOC1 will convert Channel you choose Does not have to be B1
//AdcbRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC1CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
//AdcbRegs.ADCSOC2CTL.bit.CHSEL = 2; //SOC2 will convert Channel you choose Does not have to be B2
//AdcbRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC2CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
//AdcbRegs.ADCSOC3CTL.bit.CHSEL = 3; //SOC3 will convert Channel you choose Does not have to be B3
//AdcbRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcbRegs.ADCSOC3CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC3
AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCB1
AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
//ADCD
// AdcdRegs.ADCSOC0CTL.bit.CHSEL = 0; // set SOC0 to convert pin D0
// AdcdRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcdRegs.ADCSOC0CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA will trigger SOC0
// AdcdRegs.ADCSOC1CTL.bit.CHSEL = 1; //set SOC1 to convert pin D1
// AdcdRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
// AdcdRegs.ADCSOC1CTL.bit.TRIGSEL = 13; // EPWM5 ADCSOCA will trigger SOC1
//AdcdRegs.ADCSOC2CTL.bit.CHSEL = ???; //set SOC2 to convert pin D2
//AdcdRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcdRegs.ADCSOC2CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC2
//AdcdRegs.ADCSOC3CTL.bit.CHSEL = ???; //set SOC3 to convert pin D3
//AdcdRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
//AdcdRegs.ADCSOC3CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC3
// AdcdRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to SOC1, the last converted, and it will set INT1 flag ADCD1
// AdcdRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
// AdcdRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
// Enable DACA and DACB outputs
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
DacbRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacB output-->uses ADCINA1
DacbRegs.DACCTL.bit.LOADMODE = 0; //load on next sysclk
DacbRegs.DACCTL.bit.DACREFSEL = 1; //use ADC VREF as reference voltage
EDIS;
setupSpib();
init_eQEPs();
//Exorcise 2
//EPwm2A + B
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2; //Free-run mode
EPwm2Regs.TBCTL.bit.CLKDIV = 0; //Divide by 1
EPwm2Regs.TBCTL.bit.CTRMODE = 0; //Count-up mode
EPwm2Regs.TBCTL.bit.PHSEN = 0; //Disabled phase loading
EPwm2Regs.TBCTR = 0; //Setting TBCTR to 0
EPwm2Regs.TBPRD = 2500; //50MHz -> 20KHz
EPwm2Regs.CMPA.all = 0; //Triggering every 0 SECONDS!!!
EPwm2Regs.AQCTLA.bit.CAU = 1; //Setting the PWM output low every action
EPwm2Regs.AQCTLA.bit.PRD = 2; //Setting the PWM high each period reach
EPwm2Regs.CMPB.all = 0; //Triggering every 0 SECONDS!!!
EPwm2Regs.AQCTLB.bit.CBU = 1; //Setting the PWM output low every action
EPwm2Regs.AQCTLB.bit.PRD = 2; //Setting the PWM high each period reach
EPwm2Regs.TBPHS.bit.TBPHS = 0; //Setting phase to 0
// 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_INT6;
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA CANB
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
PieCtrlRegs.PIEIER1.bit.INTx2 = 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;
PieCtrlRegs.PIEIER6.bit.INTx3 = 1; //SPiB
// ----- code for CAN start here -----
// Enable CANB in the PIE: Group 9 interrupt 7
PieCtrlRegs.PIEIER9.bit.INTx7 = 1;
// ----- code for CAN end here -----
// ----- code for CAN start here -----
// Enable the CAN interrupt signal
CanbRegs.CAN_GLB_INT_EN.bit.GLBINT0_EN = 1;
// ----- code for CAN end here -----
init_serialSCIC(&SerialC,115200);
init_serialSCID(&SerialD,115200);
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// ----- code for CAN start here -----
// // Transmit Message
// // Initialize the transmit message object used for sending CAN messages.
// // Message Object Parameters:
// // Message Object ID Number: 0
// // Message Identifier: 0x1
// // Message Frame: Standard
// // Message Type: Transmit
// // Message ID Mask: 0x0
// // Message Object Flags: Transmit Interrupt
// // Message Data Length: 4 Bytes
// //
// CANsetupMessageObject(CANB_BASE, TX_MSG_OBJ_ID, 0x1, CAN_MSG_FRAME_STD,
// CAN_MSG_OBJ_TYPE_TX, 0, CAN_MSG_OBJ_TX_INT_ENABLE,
// TX_MSG_DATA_LENGTH);
// Measured Distance from 1
// Initialize the receive message object 1 used for receiving CAN messages.
// Message Object Parameters:
// Message Object ID Number: 1
// Message Identifier: 0x060b0101
// Message Frame: Standard
// Message Type: Receive
// Message ID Mask: 0x0
// Message Object Flags: Receive Interrupt
// Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
// for a Receive mailbox)
//
CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_1, 0x060b0101, CAN_MSG_FRAME_EXT,
CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
RX_MSG_DATA_LENGTH);
// Measured Distance from 2
// Initialize the receive message object 2 used for receiving CAN messages.
// Message Object Parameters:
// Message Object ID Number: 2
// Message Identifier: 0x060b0102
// Message Frame: Standard
// Message Type: Receive
// Message ID Mask: 0x0
// Message Object Flags: Receive Interrupt
// Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
// for a Receive mailbox)
//
CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_2, 0x060b0103, CAN_MSG_FRAME_EXT,
CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
RX_MSG_DATA_LENGTH);
// Measurement Quality from 1
// Initialize the receive message object 2 used for receiving CAN messages.
// Message Object Parameters:
// Message Object ID Number: 3
// Message Identifier: 0x060b0201
// Message Frame: Standard
// Message Type: Receive
// Message ID Mask: 0x0
// Message Object Flags: Receive Interrupt
// Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
// for a Receive mailbox)
//
CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_3, 0x060b0201, CAN_MSG_FRAME_EXT,
CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
RX_MSG_DATA_LENGTH);
// Measurement Quality from 2
// Initialize the receive message object 2 used for receiving CAN messages.
// Message Object Parameters:
// Message Object ID Number: 4
// Message Identifier: 0x060b0202
// Message Frame: Standard
// Message Type: Receive
// Message ID Mask: 0x0
// Message Object Flags: Receive Interrupt
// Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
// for a Receive mailbox)
//
CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_4, 0x060b0203, CAN_MSG_FRAME_EXT,
CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
RX_MSG_DATA_LENGTH);
//
// Start CAN module operations
//
CanbRegs.CAN_CTL.bit.Init = 0;
CanbRegs.CAN_CTL.bit.CCE = 0;
// // Initialize the transmit message object data buffer to be sent
// txMsgData[0] = 0x12;
// txMsgData[1] = 0x34;
// txMsgData[2] = 0x56;
// txMsgData[3] = 0x78;
// // Loop Forever - A message will be sent once per second.
// for(;;)
// {
//
// CANsendMessage(CANB_BASE, TX_MSG_OBJ_ID, TX_MSG_DATA_LENGTH, txMsgData);
// txMsgCount++;
// DEVICE_DELAY_US(1000000);
// }
// ----- code for CAN end here -----
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"Num Timer2:%ld Num SerialRX: %ld\r\n",CpuTimer2.InterruptCount,numRXA);
serial_printf(&SerialA,"robot x:%.3f robot y:%.3f theta:%.3f\r\n", xr, yr, thp);
serial_printf(&SerialA,"target x:%.3f target y:%.3f index:%d\r\n", xstck[stck_ptr-1], ystck[stck_ptr-1], stck_ptr);
serial_printf(&SerialA,"turnerror:%.3f alpha:%.3f Norm_theta:%.3f\r\n", turnerror, alpha, theta);
serial_printf(&SerialA,"vref:%.3f turn:%.3f curr control:%d\r\n", vref, turn, ctrl);
serial_printf(&SerialA,"arrived? %d\r\n", arrived);
//serial_printf(&SerialA,"D1 %ld D2 %ld",dis_1,dis_3);
//serial_printf(&SerialA," St1 %ld St2 %ld\n\r",measure_status_1,measure_status_3);
//serial_printf(&SerialA,"rad L: %.3f rad R: %.3f\n\r",radl,radr);
//serial_printf(&SerialA,"vel L: %.3f vel R: %.3f\n\r",omegal*radius,omegar*radius);
//serial_printf(&SerialA,"a:%.3f,%.3f,%.3f g:%.3f,%.3f,%.3f\r\n",accelx,accely,accelz,gyrox,gyroy,gyroz);
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)
{
CpuTimer0.InterruptCount++;
numTimer0calls++;
// if ((numTimer0calls%50) == 0) {
// PieCtrlRegs.PIEIFR12.bit.INTx9 = 1; // Manually cause the interrupt for the SWI
// }
if ((numTimer0calls%50) == 0) {
// //displayLEDletter(LEDdisplaynum);
// LEDdisplaynum++;
// if (LEDdisplaynum == 0xFFFF) { // prevent roll over exception
// LEDdisplaynum = 0;
...
This file has been truncated, please download it to see its full contents.
user_xy.h
C Header FileThis code is our partially working X and Y waypoint coordinate control algorithm header file. This is used together with user_xy.c and is called inside the main.c file
// macros
#define MIN(A,B) (((A) < (B)) ? (A) : (B));
#define MAX(A,B) (((A) > (B)) ? (A) : (B));
/* USER_XY.C */
int xy_control(float *_vref, float *_turn,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* alpha_probe,float* theta_probe,float* turnerror_probe);
int my_xycontrol(float *_vref, float *_turn,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* alpha_probe,float* theta_probe,float* turnerror_probe);
user_xy.c
C/C++This code contains a function used for calculating the turn and vref used for driving the motors. When given inputs such as current position and heading, current vref and turn, turning thresholds and desired (x,y) coordinate, the function will calculate and update vref and turn and return 1 if the robot is currently in range of the desired waypoint and 0 if not yet.
// /*
// ____________________________________________________
// | |
// | |
// | |
// | |
// | | | |
// | | | |
// | | | |
// | | | |
// | |____________| |
// | |
// | |
// | |
// | |
// | |
// |____________________ y __________________|
// ^
// |
// |
// |------> x
// (0,0) Theta Right hand rule
// */
// // STANDARD ANSI INCLUDES
// #include <std.h> // DSP/BIOS standard include file
// #include <hwi.h>
// #include <swi.h>
// #include <log.h> // LOG_printf calls
// #include <mem.h> // MEM_alloc calls
// #include <que.h> // QUE functions
// #include <sem.h> // Semaphore functions
// #include <sys.h>
// #include <tsk.h> // TASK functions
// #include <rtdx.h> // RTDX functions
#include <math.h> // sinf,cosf,fabsf, etc.
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
// // DSP INCLUDES
// #include <c6x.h> // register defines, in c:\ti\c6000\cgtools\include
// #include <c6x11dsk.h> // TI header in the directory c:\ti\c6000\dsk6x11\include
// #include <fastrts67x.h> // TI's real-time math library, in c:\ti\c6700\mthlib\include
// // COECSL INCLUDES // all COECSL functions are usually in the directory f:\C6713DSK\include
// #include <c6xdskdigio.h> // COECSL functions for daugher card, encs, PWM
// #include <max3100uart.h> // COECSL functions for communication to max serial chip
// #include <dac7724.h> // COECSL functions for the DAC7724 chip
// #include <ad7864.h> // COECSL functions for the AD7864 chip
// #include <RCservo.h> // COECSL functions to set up PWM ch3 and ch4 to drive RC servos
// #include <switch_led.h> // COECSL functions for turning off LEDs, monitoring switches
// #include <dspvisioncolor50Hz_cLCD.h>
// #include <i2c.h>
// #include <edma.h>
// #include <sharpir.h>
// #include <dsk6713.h>
// #include <user_ColorVisionFuncs.h>
// #include <user_UARTFuncs.h>
// #include <user_PIFuncs.h>
// #include <user_IR_UltraFuncs.h>
// #include <atmel_pwrboard2.h>
// #include <color_LCD.h>
#include "user_xy.h"
#define PI 3.1415926535897932384626433832795F
#define TWOPI 6.283185307179586476925286766559F
#define HALFPI 1.5707963267948966192313216916398F
#define GRAV 9.81F
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* alpha_probe,float* theta_probe,float* turnerror_probe)
{
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 > 3.14159265358979323846 ) {
theta = thetaabs - 2.0*3.14159265358979323846 *floorf((thetaabs+3.14159265358979323846 )/(2.0*3.14159265358979323846));
} else if (thetaabs < -3.14159265358979323846 ) {
theta = thetaabs - 2.0*3.14159265358979323846 *ceilf((thetaabs-3.14159265358979323846 )/(2.0*3.14159265358979323846));
} 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);
*alpha_probe = alpha;
*theta_probe = theta;
// calculate turn error
// turnerror = theta - alpha; old way using left hand coordinate system.
turnerror = alpha - theta;
*turnerror_probe = turnerror;
// check for shortest path
if (fabsf(turnerror + 2.0*3.14159265358979323846 ) < fabsf(turnerror)) turnerror += 2.0*3.14159265358979323846 ;
else if (fabsf(turnerror - 2.0*3.14159265358979323846 ) < fabsf(turnerror)) turnerror -= 2.0*3.14159265358979323846 ;
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) > 3.14159265358979323846/2) {
dir = -dir;
}
turnerror = 0;
}
} else {
target_near = 0;
}
// vref is 1 tile/sec; but slower when close to target.
*vref_forxy = dir*MIN(dist,1);
// float Kp_turn_small = 2F;
if (fabsf(*vref_forxy) > 0.0) {
// if robot 1 tile away from target use a scaled KP value.
if (turnerror > 0.007F) *turn_forxy = (*vref_forxy*20.0F)*turnerror;
else *turn_forxy = (*vref_forxy*0.7F)*turnerror;
} else {
// normally use a Kp gain of 2
*turn_forxy = 2.0F*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);
}
int my_xycontrol(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* alpha_probe,float* theta_probe,float* turnerror_probe) {
float vectx = x_desired - x_pos;
float vecty = y_desired - y_pos;
float dist = sqrtf(vectx*vectx + vecty*vecty);
float dir = -1.0F;
int target_near = 0;
float theta = fmodf(thetaabs, TWOPI); // Wrap within [-2PI, 2PI]
// Adjust to ensure the result lies in [-PI, PI]
if (theta > PI) theta -= TWOPI;
else if (theta < -PI) theta += TWOPI;
*theta_probe = theta;
float alpha = my_atanf(vecty,vectx);
*alpha_probe = alpha;
float turnerror = alpha - theta; // Difference between desired and current angles
if (turnerror > PI) turnerror -= TWOPI;
else if (turnerror < -PI) turnerror += TWOPI;
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;
*turnerror_probe = turnerror;
float Kpv = 0.8;
float Kpt = 2;
*vref_forxy = -Kpv*dir*MIN(dist,1);
*turn_forxy = Kpt*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;
}
Thanks to Dan Block.
Comments