Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
 |
| |||||
 |
| |||||
Here is our team, from left to right, Madison, Matthew, Alex, and Weihao
The goal of the project was to create a robot that would take a line drawing and draw the same drawing on the floor. Here is an overview of the different components of our project:
The first component is to draw a collection of lines in LabVIEW and then send them to the Raspberry Pi. This was done using LabVIEW's picture functions and grabbing the position value of the user's cursor on the picture itself. While recording the points in order, LabVIEW also draws lines between them, to demonstrate what the end result should look like. We then combine the data into a string with each entry separated by spaces, which we can then send to the Raspberry Pi through a TCP Open Port connection.
LabVIEW VI
The python code on the raspberry pi reads the data as a string, splits it by spaces, splits it in half to get the x and y coordinates respectfully, and then sends the data to the C code using the serial port. The serial port reads two "#" before receiving the desired position data to distinguish it from the April Tag data which sends two "*".
Robot Localization with AprilTagThis robot uses AprilTags for robot pose estimation. AprilTags are a type of fiducial marker that is used in many computer vision applications such as camera calibration, distance measurements, object size estimation, etc. The python script on Raspberry Pi will initialize the robot position as (0, 0, 0) and detect April tags on the wall with the OpenCV library. This script will calculate the location of our robot based on the relative distance and angle measurements from our robot to the April tags on the wall. The localization output will be x, y, and z position in meters, and row, pitch, and yaw angle in radians. Then the python script will send the x and y position along with the yaw (bearing) angle to the red board through the serial port. Then, with the Kalman filter, the pose data from the AprilTags will fuse with the pose data from the wheel encoders on the robot.
However, in the lab environment, the camera had difficulty consistently and accurately detecting AprilTags while the robot is moving. One potential solution is to adjust the size of AprilTags and the lighting of the lab.
Path GenerationThe Raspberry Pi is connected to a usb camera that is able to view AprilTags which can help it calculate its location (X, Y, bearing angle). This location data is then sent to the Launchpad board. The Launchpad takes this data, along with IMU and optical encoder data of the robot location and uses a Kalman Filter to fuse speed and accuracy of the various sources into an overall position. Then, the robot takes all of this data and draws the line.
X, Y, and bearing Angle Diagram of Robot
To draw the line the robot first drives to the first point that it needs to start drawing and then uses a servo motor to put the pen down. The position in which it starts is the X = 0, Y = 0, and bearing angle = 0 so if the start of the drawing is in the top then it will move to the top to start drawing. After the robot puts the pen down it uses all of the position data to generate its position and uses the location of the end point of the line it is drawing to get a bearing that it wants to follow. Then, it turns until it gets to that bearing and when it is within.02 radians of the bearing it wants to go on it moves forward. If it ever gets off track, outside of that.04 radian window, it will turn back so that it is back within it. When it gets within.2 feet of the point it will generate the next point and try to move towards that. This threshold could be adjusted for finer points, but the robot should slow down so that it does not overshoot the point and miss.
There were two other control algorithms that were attempted in this project. Both of them are still currently in the code base, however they are commented out. The first was to use linear algebra and projections to find the distance the robot was from the line. Then it used a PID control to move the robot back to the line without overshooting it. This strategy had a lot of edge cases most of them involving when the point switched. Most of the math was only made for if the line was in the first quadrant of the unit circle. If the robot was for some reason behind the starting point of the line it could have a tendency to move away from the point. Ultimately these edge cases could cause the robot to try to move in the wrong way, so another control algorithm was developed.
This was to use a PID on the bearing similar to the actual control method used. This was a very good control algorithm for the first few points, but if it got past the point the PID loop would cause the robot to go out of control as it way overshot the bearing it wanted to be at. There were several control limitations to prevent a runaway algorithm, but they also prevented the robot from going into a full turn to get back to the point. Eventually the integral windup would occur and the robot would just start spinning uncontrollably, but if the integral component was more limited the control would not get to even the first few points because it needed to counteract the other effects like the wheels not being even.
The final control scheme was developed so that the robot would just turn until it was on the right bearing and then it would go. This was a much simpler solution and was more effective, if it could get within the right bearing.
KalmanFiltering
The optical encoders were used to generate the X and Y positions of the robot car and the Z-axis gyro IMU data was used to generate the bearing angle. The AprilTags were also used to measure these three values. The encoder and IMU data is much more responsive and consistent than the AprilTag data, but drifts over time. The AprilTag data is slow and inconsistently sends data, but does not have the drift that the encoders have. So, it can be used to reset the position after enough drift has accumulated. The fusion of the two sets of data with a Kalman filter would theoretically give position data that is responsive with limited drift. The way that the Kalman filter works is by averaging the X, Y, and bearing angle values with weights based on the variance of each of them. The smaller the variance the the larger the weight associated with it. It consists of a prediction step and a correction step. In the prediction step, integration using velocities is used to and current states is used to generate an estimate for the position one time step ahead. In the correction step, these predictions are evaluated against measurements from the AprilTags to find and error between them. Overall, we found that the encoder and IMU data gave generally good estimates of the robot's location. However, the AprilTag data was very lagged and was not as accurate or consistent as we anticipated. This meant we set the trust with the AprilTag data very low (high variance) and the trust with the encoder and IMU data high (small variance).
FailuresUltimately, the robot car was unable to properly copy the drawing sent from LabView. We believe that this failure stemmed from multiple errors including not being able to consistently read the AprilTags on the walls, a long delay for receiving the AprilTag data, errors calibrating both the AprilTag data and the covariance in the Kalman filter, and overshoot/integral windup issues with the PID controller.
// hi this is to reduce the problems with version control with several people
// I am going to put my files in this so that it will reduce the chances of git conflicts - Alex
//------------------------alex functions--------------------
#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 <control.h>
/*
// here are the variables
extern float cur_x_e;
extern float cur_y_e;
extern float cur_bearing_e;
extern float cur_vel_x_e;
extern float cur_vel_y_e;
extern float cur_omega_in_e;
extern float xarray[];
extern float yarray[];
float eturn;
float turn;
float eturn_I;
float vel_right;
float vel_left;
float eturn_I_1;
float zero_x;
float zero_y;
float shift_pos_x;
float shift_pos_y;
float shift_line_x2;
float shift_line_y2;
float ux;
float uy;
float top;
float bottom;
float proj_x;
float proj_y;
float comp_x;
float comp_y;
float slope_line;
float slope;
float slope_robot_line;
float eturn_d;
float omega;
float eturn_1;
float k_turn_I = .8;// this will need to be tuned to damp the system
float k_turn_p = .7; // tune for pid;
float eturn_p;
float k_turn_d = .05; // tune for the d
int satR;
int satL;
struct position {
float x;
float y;
float bearing;
float omega;
float velx;
float vely;
float dist_to_line;
int direction_to_line;// boolean left of the robot is positive
float distance_along_line;
};
struct line {
float line_global_heading;
float x1;
float y1;
float x2;
float y2;
float length;
int index_line;
int pen_down_bool;
};
float aim(struct position pos, struct line prev_line);// this function will generate the angle that the wheels should be
void gen_line(float x_2, float y_2);// this function will generate the line that I want to follow
//void set_wheel_speeds(float turn_angle);
void set_pen_down(int pen_down);
void update_pos(float cur_x, float cur_y, float cur_bearing, float cur_vel_x, float cur_vel_y, float omega_in);
float set_velocity = 3.0; // this is the baseline velocity that the wheels will be trying to get too
int go();// this position will make the wheels run, it will return 1 when it is "close enough" to the point 0 is running.
struct position cur_pos;
struct line cur_line;
struct line prev_line;
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
//float turn_angle(struct position pos, float turn_current, float[] turn_error, struct line cur_line) {
// here will be a pid loop to get back on track
// there will be a constant set velocity and then turn will increase or decrease the velocities of the wheels to turn
//} */
float set_velocity = .7; // this is the baseline velocity that the wheels will be trying to get too
float cur_x_e = 0;
float cur_y_e = 0;
float cur_bearing_e = 0;
float cur_vel_x_e = 0;
float cur_vel_y_e = 0;
float cur_omega_in_e = 0;
int last_line_index = 25;
int ready = 0;
extern float eturn;
extern float turn;
extern float eturn_I;
extern float vel_right;
extern float vel_left;
extern float eturn_I_1;
extern float zero_x;
extern float zero_y;
extern float shift_pos_x;
extern float shift_pos_y;
extern float shift_line_x2;
extern float shift_line_y2;
extern float ux;
extern float uy;
extern float top;
extern float bottom;
extern float proj_x;
extern float proj_y;
extern float comp_x;
extern float comp_y;
extern float slope_line;
extern float slope;
extern float slope_robot_line;
extern float eturn_d;
extern float omega;
extern float eturn_1;
//extern float k_turn_I;// this will need to be tuned to damp the system
//extern float k_turn_p; // tune for pid;
extern float eturn_p;
extern float k_turn_d; // tune for the d
extern float Kp;
extern float Ki;
extern int satR;
extern int satL;
extern float xarray_i[100];
extern float yarray_i[100];
extern float VLeftK;
extern float VRightK;
float evel_left = 0.0;
float evel_right = 0.0;
float IrightK = 0.0;
float IrightK_1 = 0.0;
float evel_right_1 = 0.0;
float IleftK = 0.0;
float IleftK_1 = 0.0;
float evel_left_1 = 0.0;
float distance_to_point = 0.0;
float k_turn_I = 2;
float k_turn_p = 1.0;
float kp_vel = 2;
float ki_vel = 10;
float k_overall_turn = 2.0;
struct position cur_pos;
struct line cur_line;
struct line prev_line;
int turn_left = 0;
int turn_right = 0;
float hard_vell = 0.0;
float hard_velr = 0.0;
int time_out_count = 0;
int go(void) {
update_pos(cur_x_e, cur_y_e, cur_bearing_e, cur_vel_x_e, cur_vel_y_e, cur_omega_in_e); // figure out where we are
//set_pen_down(cur_line.pen_down_bool);
if(!ready) {// wait for the lines to be sent over
set_pen_down(0); // keep pen up
return 0;
}
set_pen_down(1); // puts the pen down
if(cur_line.index_line > 25) { // this statment should be catching us at the end
setEPWM2B(0.0);// stopping the robot
setEPWM2A(0.0);
set_pen_down(0); // putting the pen up
return 0;
}
if(cur_pos.distance_to_pos < .05) { // if we are close to the point we are trying to get to we should make another point
// if we are in this statment we are at the end of the line
setEPWM2B(0.0);
setEPWM2A(0.0);
//DELAY_US(3000000); //delay in microseconds so 3 seconds is 3 000 000
gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]);//make another point
//aim(cur_pos, cur_line);
//set_pen_down(1);
return 1;
}
if((cur_pos.bearing_wanted - cur_pos.bearing) < .02 & ((cur_pos.bearing_wanted - cur_pos.bearing) > -.02)) { // if we are close to the right direction
turn_left = 0;// dont turn left or right
turn_right = 0;
} else if ((cur_pos.bearing_wanted - cur_pos.bearing) > 0) { // if our bearing wanted is greater than the bearing we have turn right
turn_right = 1;// turn right
turn_left = 0;
} else {
turn_right = 0;
turn_left = 1; // we turn left
}
time_out_count = time_out_count + 1; // time out counter so that every 3 seconds we change targets in case we get stuck
if(time_out_count == 750) {
time_out_count = 0;
gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]); // changes the target
}
if (turn_left == 1) { // if turn left turn left
//setEPWM2B(-2.0);
hard_vell = -4;
//setEPWM2A(-2.0);
hard_velr = -4;
} else if (turn_right == 1) { // if turn right turn right
//setEPWM2B(2.0);
hard_vell = 4;
hard_velr = 4;
//setEPWM2A(2.0);
} else { // else straight
//setEPWM2B(-2.0);
hard_vell = -1.7;
hard_velr = 1.5;
//setEPWM2A(2.0);
}
setEPWM2B(hard_vell); // left epwm
setEPWM2A(hard_velr); // right epwm
return 1;
/* below is all of the code for the second control algorithm. PID on bearing
} else if (cur_line.length == 0 ) { // if this statment is true the x2 and x1 and y2 and y1 are the same
set_pen_down(0);
gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]);
return 0;
}
else if(cur_pos.distance_to_pos < .2) {
// if we are in this statment we are at the end of the line
setEPWM2B(0.0);
setEPWM2A(0.0);
//DELAY_US(3000000); //delay in microseconds so 3 seconds is 3 000 000
gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]);
//aim(cur_pos, cur_line);
//set_pen_down(1);
return 1;
} else { // we are driving I guess
//setEPWM2B(0.0);
//setEPWM2A(0.0);
set_pen_down(cur_line.pen_down_bool);
aim(cur_pos, cur_line);
return 1;
} */
//aim2(cur_pos.bearing_wanted, cur_pos.distance_to_pos);
//gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]);
}
// to do- make a turn for while the system is at the end of the line
float aim(struct position pos, struct line the_line) {
// here will be a PID loop and the activation for the motors
//PosLeft_k_1 = PosLeft_k;// all of the code in this section is setting the previous values of all of the needed values so we can integrate or derive
//PosRight_k_1 = PosRight_k;
//eLeft_k_1 = eLeft_k;
//eRight_k_1 = eRight_k;
//ILeft_k_1 = ILeft_k;
//IRight_k_1 = IRight_k;
//LeftWheel_k_1 = LeftWheel;
//RightWheel_k_1 = RightWheel;
//xdot_k_1 = xdot;
//ydot_k_1 = ydot;
//LeftWheel = readEncLeft();// sets left wheel distance (technically total amt of angle change)
//RightWheel = readEncRight(); // sets left wheel distance (technically total amt of angle change)
//wLeftK = (LeftWheel- LeftWheel_k_1) / 0.004; // calculates velocity based on time difference and previous position
//wRightK = (RightWheel - RightWheel_k_1) / 0.004; // same here but right
//PosLeft_k = LeftWheel / 5.128; //converts from rad to ft
//PosRight_k = RightWheel / 5.128; // doing it again
//bearing = .1946/.56759*(RightWheel - LeftWheel); // this finds the "compass" direction. but it is really just the angle change since beginning
//theta_avg_k = .5*(RightWheel + LeftWheel); // finds the average distance of the robot
//omega_avg_k = .5*(wRightK + wLeftK); // finds the avg velocity of the robot
//xdot = .19460*omega_avg_k*cos(bearing); // finds the x velocity of the robot
//ydot = .19460*omega_avg_k*sin(bearing); // finds the yvelocity of the robot
//globalx = globalx + .004*(xdot+xdot_k_1)/2; // this is global x coordinate with starting pos as 0
//globaly = globaly + .004*(ydot+ydot_k_1)/2; // global y coordinate with starting pos as 0
//VLeftK = (PosLeft_k - PosLeft_k_1) / 0.004; // calculates velocity based on time difference and previous position
//VRightK = (PosRight_k - PosRight_k_1) / 0.004;
//eturn = (VLeftK - VRightK) + turn; // error but the turn is to help with turning
//eLeft_k = set_velocity - Kturn*eturn;//- VLeftK ; // error left
//eRight_k = set_velocity + Kturn*eturn;//- VRightK; // error right (to be used with pid)
//if (satR) {
// don't run the else so that we stop summing no more integral windup
//} else {
//IRight_k = IRight_k_1 + 0.004 * (eRight_k + eRight_k_1) / 2; // integrate for PID
//}
//if (satL) {
// dont run the else so that we stop summing
//} else {
// ILeft_k = ILeft_k_1 + 0.004 * (eLeft_k + eLeft_k_1) / 2; // integrate for PID
//}
//uLeft = Kp * eLeft_k + Ki * ILeft_k; // PI for left wheel
//uRight = Kp * eRight_k + Ki * IRight_k; // PI for right wheel
//if ((uRight < 10) & (uRight > -10)) { // this is code to make sure that we don't request too much
// satR = 0; // reset the satR
//}
//if ((uLeft < 10) & (uLeft > -10)) {
// satL = 0; // reset the satL
//}
//eturn = cur_pos.dist_to_line * (cur_pos.direction_to_line * (-1.0)); // essentially distance back to line but has direction
//eturn = cur_pos.bearing_wanted - cur_pos.bearing; // current eturn is going to be the difference between the bearing wanted and what is had
eturn = (VLeftK - VRightK) + (cur_pos.bearing_wanted - cur_pos.bearing);
//eturn_d = -omega;
eturn_I = (eturn+eturn_1)/2*.004 + eturn_I_1; // using trapezoid rule to get integral of error terms
turn = k_turn_I*eturn_I + k_turn_p*(eturn_p);// + k_turn_d*eturn_d;
if (distance_to_point < 1) {
set_velocity = distance_to_point;
} else {
set_velocity = 3;
}
evel_left = set_velocity - VLeftK - k_overall_turn*turn; // can flip these
evel_right = set_velocity - VRightK + k_overall_turn*turn;
IrightK = IrightK_1 + (evel_right + evel_right_1)/2*.004;
IleftK = IleftK_1 + (evel_left + evel_left_1)/2*.004;
//eturn = (VLeftK - VRightK) + turn; // error but the turn is to help with turning
//eLeft_k = set_velocity - Kturn*eturn- VLeftK ; // error left
//eRight_k = set_velocity + Kturn*eturn- VRightK; // error right (to be used with pid)
vel_left = kp_vel * evel_left + ki_vel * IleftK; // PI for left wheel
vel_right = kp_vel * evel_right + ki_vel * IrightK; // PI for right wheel
if (turn > 5) {
turn = 5;
} else if (turn < -5) {
turn = -5;
}
if (eturn_I > 5) {
eturn_I = 5;
} else if (eturn_I < -5) {
eturn_I = -5;
}
if (vel_left >= 7.0) {
vel_left = 7.0;
} else if (vel_left <= -7.0) {
vel_left = -7.0;
}
if (vel_right >= 7.0) {
vel_right = 7.0;
} else if (vel_right <= -7.0) {
vel_right = -7.0;
}
if (IleftK >= 15) {
IleftK = 15;
} else if (IleftK <= -15) {
IleftK = -15;
}
if (IrightK >= 15) {
IrightK = 15;
} else if (IrightK <= -15) {
IrightK = -15;
}
// change of plan is being done here
// need to use a PID on the robots bearing
// bearing_wanted should be the global bearing towards the target
setEPWM2B(-vel_left/2); //maddie added a negative
setEPWM2A(vel_right/2);
eturn_1 = eturn;
eturn_I_1 = eturn_I;
IleftK_1 = IleftK;
IrightK_1 = IrightK;
evel_right_1 = evel_right;
evel_left_1 = evel_left;
return turn;
}
void gen_line(float x_2, float y_2) {
cur_line.x1 = prev_line.x2; // set the current line x value equal to the previous line second x val
cur_line.y1 = prev_line.y2; // set the current line y start to the previous line y end
cur_line.x2 = x_2; // set the current line x final to the value it wants
cur_line.y2 = y_2; // set the current line y value equal to what it wants
// now to get bearing in degrees
//slope = (cur_line.y2 - cur_line.y1)/(cur_line.x2- cur_line.x1); // this fines the global slope of the current line
//cur_line.line_global_heading = atan(slope); // this fines the global heading of the line in degrees
cur_line.length = sqrt(((cur_line.x2-cur_line.x1)*(cur_line.x2-cur_line.x1)) + ((cur_line.y2 - cur_line.y1)*(cur_line.y2 - cur_line.y1)));
// the above line finds the length from the start to the finish of the line
cur_line.index_line = prev_line.index_line + 1;// this line adds one to the starting value of the previous line
if (prev_line.index_line == 0) { // just recall that the index starts at 1 for the first line but the previous
// line will be initialized with all 0's
// this function should be changed for multiple lines
cur_line.pen_down_bool = 0; // the pen is up
} else if(prev_line.index_line == last_line_index) {
cur_line.pen_down_bool = 0; // the pen is up if it is the last line
} else {
cur_line.pen_down_bool = 1; // the pen is down
}
prev_line = cur_line; // set the previous line equal to the new line
}
void update_pos(float cur_x, float cur_y, float cur_bearing, float cur_vel_x, float cur_vel_y, float omega_in) {
//zero_x = cur_line.x1; // this sets where our local 0 is
//zero_y = cur_line.y1; // this sets where our local 0 is
// this bout to get mathy
//https://textbooks.math.gatech.edu/ila/projections.html
cur_pos.omega = omega_in; // next lines are getting matt code date
cur_pos.x = cur_x;
cur_pos.y = cur_y;
cur_pos.bearing = cur_bearing;
cur_pos.velx = cur_vel_x;
cur_pos.vely = cur_vel_y;
//shift_pos_x = cur_pos.x - zero_x; // this is now the current local position
//shift_pos_y = cur_pos.y - zero_y; // this is now the current local position
//shift_line_x2 = cur_line.x2 - zero_x; // this is the local end value
//shift_line_y2 = cur_line.x2 - zero_x; // this is the local end value
//if((shift_line_x2 == 0) & (shift_line_y2 == 0)) {
//gen_line(xarray_i[cur_line.index_line + 1], yarray_i[cur_line.index_line + 1]);
// return ; // if the line value lenght is 0
//}
//need to (vec(u).vec(x)/vec(u).vec(u))vec(u)
// this is to get the projection of the point onto the line
// will write as top/bottom * ux and top/bottom * uy
// u is the unit vector along the line
//ux = shift_line_x2/cur_line.length; // this is the local direction vector
//uy = shift_line_y2/cur_line.length; // this is the local direction vector
// theoretically the ux^2 + uy^2 = 1 but thats lin alg
//top = ux*shift_pos_x + uy*shift_pos_y; // for projection it is the dot product
//bottom = ux*ux + uy*uy;
// proj_x = top/bottom*ux;
// proj_y = top/bottom*uy;
//cur_pos.distance_along_line = sqrt(proj_x*proj_x + proj_y*proj_y); // this is the length of the projection
//comp_x = shift_pos_x - proj_x; // this is used for the distance to the line
//comp_y = shift_pos_y - proj_y; // this is used for the distance to the line
//cur_pos.dist_to_line = sqrt(comp_x*comp_x + comp_y*comp_y); // this finds the distance to the line
// this is a little weird to get which side it is on
// This is also the most likely point for errors
// we need to find whether the line is on the right side or left side of the robot
// i will use the direction of the line
// then I will check whether the line from the robots current pos to the end of the line is more negative
//slope_line = (cur_line.y2 - cur_line.y1)/(cur_line.x2 - cur_line.x1);
//slope_robot_line = (cur_line.y2 - cur_pos.y)/(cur_line.x2 - cur_pos.x);
//cur_pos.direction_to_line = (slope_line <= slope_robot_line);//left is 1 right is 0
//imagine a vector or a line to both the position and to the final pos of line
//cur_pos.direction_to_line = -1; // set to get rid of the value
//cur_pos.distance_to_line = 0.0;
//cur_pos.distance_along_line = 0.0;
cur_pos.bearing_wanted = atan2((cur_line.y2-cur_pos.y),(cur_line.x2 - cur_pos.x)) - 3.14159265/2;
cur_pos.distance_to_pos = sqrt(((cur_pos.x - cur_line.x2)*(cur_pos.x- cur_line.x2)) + ((cur_pos.y-cur_line.y2)*(cur_pos.y - cur_line.y2)));
//cur_pos.direction_to_l
}
//void set_wheel_speeds(float turn_angle) {
//}
}
}
/* SERIAL.C: This code is designed to act as a low-level serial driver for
higher-level programming. Ideally, one could simply call init_serial()
to initialize the serial port, then use serial_send("data", 4) to send
an array of data (8-bit unsigned character strings).
WRITTEN BY : Paul Miller <pamiller@uiuc.edu>
$Id: serial.c,v 1.4 2003/08/08 16:08:56 paul Exp $
*/
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h" // Device Headerfile and Examples Include File
#include <buffer.h>
#include <F28379dSerial.h>
#include <F2837xD_sci.h>
serialSCIA_t SerialA;
serialSCIB_t SerialB;
serialSCIC_t SerialC;
serialSCID_t SerialD;
char RXAdata = 0;
char RXBdata = 0;
char RXCdata = 0;
char RXDdata = 0;
uint32_t numRXA = 0;
uint32_t numRXB = 0;
uint32_t numRXC = 0;
uint32_t numRXD = 0;
float cx = 0;
float cy = 0;
float cbearing = 0;
//for serial com with pi
typedef union {
uint16_t parts[2];
float value;
} float_int;
uint16_t com_state = 0;
char send_sci[40]; //send the float
float_int to_send[10];
uint16_t received_count = 0;
uint16_t opti_count = 0;
float_int received_pi[60];
char temp_MSB = 0;
extern float xarray[25];
extern float yarray[25];
extern float apriltag;
// for SerialA
uint16_t init_serialSCIA(serialSCIA_t *s, uint32_t baud)
{
volatile struct SCI_REGS *sci;
uint32_t clk;
if (s == &SerialA) {
sci = &SciaRegs;
s->sci = sci;
init_bufferSCIA(&s->TX);
GPIO_SetupPinMux(43, GPIO_MUX_CPU1, 15);
GPIO_SetupPinOptions(43, GPIO_INPUT, GPIO_PULLUP);
GPIO_SetupPinMux(42, GPIO_MUX_CPU1, 15);
GPIO_SetupPinOptions(42, GPIO_OUTPUT, GPIO_PUSHPULL);
} else {
return 1;
}
/* init for standard baud,8N1 comm */
sci->SCICTL1.bit.SWRESET = 0; // init SCI state machines and opt flags
sci->SCICCR.all = 0x0;
sci->SCICTL1.all = 0x0;
sci->SCICTL2.all = 0x0;
sci->SCIPRI.all = 0x0;
clk = LSPCLK_HZ; // set baud rate
clk /= baud*8;
clk--;
sci->SCILBAUD.all = clk & 0xFF;
sci->SCIHBAUD.all = (clk >> 8) & 0xFF;
sci->SCICCR.bit.SCICHAR = 0x7; // (8) 8 bits per character
sci->SCICCR.bit.PARITYENA = 0; // (N) disable party calculation
sci->SCICCR.bit.STOPBITS = 0; // (1) transmit 1 stop bit
sci->SCICCR.bit.LOOPBKENA = 0; // disable loopback test
sci->SCICCR.bit.ADDRIDLE_MODE = 0; // idle-line mode (non-multiprocessor SCI comm)
sci->SCIFFCT.bit.FFTXDLY = 0; // TX: zero-delay
sci->SCIFFTX.bit.SCIFFENA = 1; // enable SCI fifo enhancements
sci->SCIFFTX.bit.TXFIFORESET = 0;
sci->SCIFFTX.bit.TXFFIL = 0x0;// TX: fifo interrupt at all levels ???? is this correct
sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
sci->SCIFFTX.bit.TXFIFORESET = 1;
sci->SCIFFRX.bit.RXFIFORESET = 0; // RX: fifo reset
sci->SCIFFRX.bit.RXFFINTCLR = 1; // RX: clear interrupt flag
sci->SCIFFRX.bit.RXFFIENA = 1; // RX: enable fifo interrupt
sci->SCIFFRX.bit.RXFFIL = 0x1; // RX: fifo interrupt
sci->SCIFFRX.bit.RXFIFORESET = 1; // RX: re-enable fifo
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.TXWAKE = 0;
sci->SCICTL1.bit.SLEEP = 0; // disable sleep mode
sci->SCICTL1.bit.RXENA = 1; // enable SCI receiver
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL1.bit.TXENA = 1; // enable SCI transmitter
sci->SCICTL1.bit.SWRESET = 1; // re-enable SCI
/* enable PIE interrupts */
if (s == &SerialA) {
PieCtrlRegs.PIEIER9.bit.INTx1 = 1;
PieCtrlRegs.PIEIER9.bit.INTx2 = 1;
IER |= (M_INT9);
PieCtrlRegs.PIEACK.all = (PIEACK_GROUP9);
}
return 0;
}
void uninit_serialSCIA(serialSCIA_t *s)
{
volatile struct SCI_REGS *sci = s->sci;
/* disable PIE interrupts */
if (s == &SerialA) {
PieCtrlRegs.PIEIER9.bit.INTx1 = 0;
PieCtrlRegs.PIEIER9.bit.INTx2 = 0;
IER &= ~M_INT9;
}
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.RXENA = 0; // disable SCI receiver
sci->SCICTL1.bit.TXENA = 0; // disable SCI transmitter
}
/***************************************************************************
* SERIAL_SEND()
*
* "User level" function to send data via serial. Return value is the
* length of data successfully copied to the TX buffer.
***************************************************************************/
uint16_t serial_sendSCIA(serialSCIA_t *s, char *data, uint16_t len)
{
uint16_t i = 0;
if (len && s->TX.size < BUF_SIZESCIA) {
for (i = 0; i < len; i++) {
if (buf_writeSCIA_1(&s->TX, data[i] & 0x00FF) != 0) break;
}
s->sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
s->sci->SCIFFTX.bit.TXFFIENA = 1; // TX: enable fifo interrupt
}
return i;
}
// For SerialB
uint16_t init_serialSCIB(serialSCIB_t *s, uint32_t baud)
{
volatile struct SCI_REGS *sci;
uint32_t clk;
if (s == &SerialB) {
sci = &ScibRegs;
s->sci = sci;
init_bufferSCIB(&s->TX);
GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(15, GPIO_INPUT, GPIO_PULLUP);
GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(14, GPIO_OUTPUT, GPIO_PUSHPULL);
} else {
return 1;
}
/* init for standard baud,8N1 comm */
sci->SCICTL1.bit.SWRESET = 0; // init SCI state machines and opt flags
sci->SCICCR.all = 0x0;
sci->SCICTL1.all = 0x0;
sci->SCICTL2.all = 0x0;
sci->SCIPRI.all = 0x0;
clk = LSPCLK_HZ; // set baud rate
clk /= baud*8;
clk--;
sci->SCILBAUD.all = clk & 0xFF;
sci->SCIHBAUD.all = (clk >> 8) & 0xFF;
sci->SCICCR.bit.SCICHAR = 0x7; // (8) 8 bits per character
sci->SCICCR.bit.PARITYENA = 0; // (N) disable party calculation
sci->SCICCR.bit.STOPBITS = 0; // (1) transmit 1 stop bit
sci->SCICCR.bit.LOOPBKENA = 0; // disable loopback test
sci->SCICCR.bit.ADDRIDLE_MODE = 0; // idle-line mode (non-multiprocessor SCI comm)
sci->SCIFFCT.bit.FFTXDLY = 0; // TX: zero-delay
sci->SCIFFTX.bit.SCIFFENA = 1; // enable SCI fifo enhancements
sci->SCIFFTX.bit.TXFIFORESET = 0;
sci->SCIFFTX.bit.TXFFIL = 0x0;// TX: fifo interrupt at all levels ???? is this correct
sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
sci->SCIFFTX.bit.TXFIFORESET = 1;
sci->SCIFFRX.bit.RXFIFORESET = 0; // RX: fifo reset
sci->SCIFFRX.bit.RXFFINTCLR = 1; // RX: clear interrupt flag
sci->SCIFFRX.bit.RXFFIENA = 1; // RX: enable fifo interrupt
sci->SCIFFRX.bit.RXFFIL = 0x1; // RX: fifo interrupt
sci->SCIFFRX.bit.RXFIFORESET = 1; // RX: re-enable fifo
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.TXWAKE = 0;
sci->SCICTL1.bit.SLEEP = 0; // disable sleep mode
sci->SCICTL1.bit.RXENA = 1; // enable SCI receiver
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL1.bit.TXENA = 1; // enable SCI transmitter
sci->SCICTL1.bit.SWRESET = 1; // re-enable SCI
/* enable PIE interrupts */
if (s == &SerialB) {
PieCtrlRegs.PIEIER9.bit.INTx3 = 1;
PieCtrlRegs.PIEIER9.bit.INTx4 = 1;
IER |= (M_INT9);
PieCtrlRegs.PIEACK.all = (PIEACK_GROUP9);
}
return 0;
}
void uninit_serialSCIB(serialSCIB_t *s)
{
volatile struct SCI_REGS *sci = s->sci;
/* disable PIE interrupts */
if (s == &SerialB) {
PieCtrlRegs.PIEIER9.bit.INTx3 = 0;
PieCtrlRegs.PIEIER9.bit.INTx4 = 0;
IER &= ~M_INT9;
}
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.RXENA = 0; // disable SCI receiver
sci->SCICTL1.bit.TXENA = 0; // disable SCI transmitter
}
/***************************************************************************
* SERIAL_SEND()
*
* "User level" function to send data via serial. Return value is the
* length of data successfully copied to the TX buffer.
***************************************************************************/
uint16_t serial_sendSCIB(serialSCIB_t *s, char *data, uint16_t len)
{
uint16_t i = 0;
if (len && s->TX.size < BUF_SIZESCIB) {
for (i = 0; i < len; i++) {
if (buf_writeSCIB_1(&s->TX, data[i] & 0x00FF) != 0) break;
}
s->sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
s->sci->SCIFFTX.bit.TXFFIENA = 1; // TX: enable fifo interrupt
}
return i;
}
// for SerialC
uint16_t init_serialSCIC(serialSCIC_t *s, uint32_t baud)
{
volatile struct SCI_REGS *sci;
uint32_t clk;
if (s == &SerialC) {
sci = &ScicRegs;
s->sci = sci;
init_bufferSCIC(&s->TX);
GPIO_SetupPinMux(139, GPIO_MUX_CPU1, 6);
GPIO_SetupPinOptions(139, GPIO_INPUT, GPIO_PULLUP);
GPIO_SetupPinMux(56, GPIO_MUX_CPU1, 6);
GPIO_SetupPinOptions(56, GPIO_OUTPUT, GPIO_PUSHPULL);
} else {
return 1;
}
/* init for standard baud,8N1 comm */
sci->SCICTL1.bit.SWRESET = 0; // init SCI state machines and opt flags
sci->SCICCR.all = 0x0;
sci->SCICTL1.all = 0x0;
sci->SCICTL2.all = 0x0;
sci->SCIPRI.all = 0x0;
clk = LSPCLK_HZ; // set baud rate
clk /= baud*8;
clk--;
sci->SCILBAUD.all = clk & 0xFF;
sci->SCIHBAUD.all = (clk >> 8) & 0xFF;
sci->SCICCR.bit.SCICHAR = 0x7; // (8) 8 bits per character
sci->SCICCR.bit.PARITYENA = 0; // (N) disable party calculation
sci->SCICCR.bit.STOPBITS = 0; // (1) transmit 1 stop bit
sci->SCICCR.bit.LOOPBKENA = 0; // disable loopback test
sci->SCICCR.bit.ADDRIDLE_MODE = 0; // idle-line mode (non-multiprocessor SCI comm)
sci->SCIFFCT.bit.FFTXDLY = 0; // TX: zero-delay
sci->SCIFFTX.bit.SCIFFENA = 1; // enable SCI fifo enhancements
sci->SCIFFTX.bit.TXFIFORESET = 0;
sci->SCIFFTX.bit.TXFFIL = 0x0;// TX: fifo interrupt at all levels ???? is this correct
sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
sci->SCIFFTX.bit.TXFIFORESET = 1;
sci->SCIFFRX.bit.RXFIFORESET = 0; // RX: fifo reset
sci->SCIFFRX.bit.RXFFINTCLR = 1; // RX: clear interrupt flag
sci->SCIFFRX.bit.RXFFIENA = 1; // RX: enable fifo interrupt
sci->SCIFFRX.bit.RXFFIL = 0x1; // RX: fifo interrupt
sci->SCIFFRX.bit.RXFIFORESET = 1; // RX: re-enable fifo
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.TXWAKE = 0;
sci->SCICTL1.bit.SLEEP = 0; // disable sleep mode
sci->SCICTL1.bit.RXENA = 1; // enable SCI receiver
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL1.bit.TXENA = 1; // enable SCI transmitter
sci->SCICTL1.bit.SWRESET = 1; // re-enable SCI
/* enable PIE interrupts */
if (s == &SerialC) {
PieCtrlRegs.PIEIER8.bit.INTx5 = 1;
PieCtrlRegs.PIEIER8.bit.INTx6 = 1;
PieCtrlRegs.PIEACK.all = (PIEACK_GROUP8);
IER |= (M_INT8);
}
return 0;
}
void uninit_serialSCIC(serialSCIC_t *s)
{
volatile struct SCI_REGS *sci = s->sci;
/* disable PIE interrupts */
if (s == &SerialC) {
PieCtrlRegs.PIEIER8.bit.INTx5 = 0;
PieCtrlRegs.PIEIER8.bit.INTx6 = 0;
IER &= ~M_INT8;
}
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.RXENA = 0; // disable SCI receiver
sci->SCICTL1.bit.TXENA = 0; // disable SCI transmitter
}
/***************************************************************************
* SERIAL_SEND()
*
* "User level" function to send data via serial. Return value is the
* length of data successfully copied to the TX buffer.
***************************************************************************/
uint16_t serial_sendSCIC(serialSCIC_t *s, char *data, uint16_t len)
{
uint16_t i = 0;
if (len && s->TX.size < BUF_SIZESCIC) {
for (i = 0; i < len; i++) {
if (buf_writeSCIC_1(&s->TX, data[i] & 0x00FF) != 0) break;
}
s->sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
s->sci->SCIFFTX.bit.TXFFIENA = 1; // TX: enable fifo interrupt
}
return i;
}
// For Serial D
uint16_t init_serialSCID(serialSCID_t *s, uint32_t baud)
{
volatile struct SCI_REGS *sci;
uint32_t clk;
if (s == &SerialD) {
sci = &ScidRegs;
s->sci = sci;
init_bufferSCID(&s->TX);
GPIO_SetupPinMux(105, GPIO_MUX_CPU1, 6);
GPIO_SetupPinOptions(105, GPIO_INPUT, GPIO_PULLUP);
GPIO_SetupPinMux(104, GPIO_MUX_CPU1, 6);
GPIO_SetupPinOptions(104, GPIO_OUTPUT, GPIO_PUSHPULL);
}
else {
return 1;
}
/* init for standard baud,8N1 comm */
sci->SCICTL1.bit.SWRESET = 0; // init SCI state machines and opt flags
sci->SCICCR.all = 0x0;
sci->SCICTL1.all = 0x0;
sci->SCICTL2.all = 0x0;
sci->SCIPRI.all = 0x0;
clk = LSPCLK_HZ; // set baud rate
clk /= baud*8;
clk--;
sci->SCILBAUD.all = clk & 0xFF;
sci->SCIHBAUD.all = (clk >> 8) & 0xFF;
sci->SCICCR.bit.SCICHAR = 0x7; // (8) 8 bits per character
sci->SCICCR.bit.PARITYENA = 0; // (N) disable party calculation
sci->SCICCR.bit.STOPBITS = 0; // (1) transmit 1 stop bit
sci->SCICCR.bit.LOOPBKENA = 0; // disable loopback test
sci->SCICCR.bit.ADDRIDLE_MODE = 0; // idle-line mode (non-multiprocessor SCI comm)
sci->SCIFFCT.bit.FFTXDLY = 0; // TX: zero-delay
sci->SCIFFTX.bit.SCIFFENA = 1; // enable SCI fifo enhancements
sci->SCIFFTX.bit.TXFIFORESET = 0;
sci->SCIFFTX.bit.TXFFIL = 0x0;// TX: fifo interrupt at all levels ???? is this correct
sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
sci->SCIFFTX.bit.TXFIFORESET = 1;
sci->SCIFFRX.bit.RXFIFORESET = 0; // RX: fifo reset
sci->SCIFFRX.bit.RXFFINTCLR = 1; // RX: clear interrupt flag
sci->SCIFFRX.bit.RXFFIENA = 1; // RX: enable fifo interrupt
sci->SCIFFRX.bit.RXFFIL = 0x1; // RX: fifo interrupt
sci->SCIFFRX.bit.RXFIFORESET = 1; // RX: re-enable fifo
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.TXWAKE = 0;
sci->SCICTL1.bit.SLEEP = 0; // disable sleep mode
sci->SCICTL1.bit.RXENA = 1; // enable SCI receiver
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL1.bit.TXENA = 1; // enable SCI transmitter
sci->SCICTL1.bit.SWRESET = 1; // re-enable SCI
/* enable PIE interrupts */
if (s == &SerialD) {
PieCtrlRegs.PIEIER8.bit.INTx7 = 1;
PieCtrlRegs.PIEIER8.bit.INTx8 = 1;
PieCtrlRegs.PIEACK.all = (PIEACK_GROUP8);
IER |= (M_INT8);
}
return 0;
}
void uninit_serialSCID(serialSCID_t *s)
{
volatile struct SCI_REGS *sci = s->sci;
/* disable PIE interrupts */
if (s == &SerialD) {
PieCtrlRegs.PIEIER8.bit.INTx7 = 0;
PieCtrlRegs.PIEIER8.bit.INTx8 = 0;
IER &= ~M_INT8;
}
sci->SCICTL1.bit.RXERRINTENA = 0; // disable receive error interrupt
sci->SCICTL2.bit.RXBKINTENA = 0; // disable receiver/error interrupt
sci->SCICTL2.bit.TXINTENA = 0; // disable transmitter interrupt
sci->SCICTL1.bit.RXENA = 0; // disable SCI receiver
sci->SCICTL1.bit.TXENA = 0; // disable SCI transmitter
}
/***************************************************************************
* SERIAL_SEND()
*
* "User level" function to send data via serial. Return value is the
* length of data successfully copied to the TX buffer.
***************************************************************************/
uint16_t serial_sendSCID(serialSCID_t *s, char *data, uint16_t len)
{
uint16_t i = 0;
if (len && s->TX.size < BUF_SIZESCID) {
for (i = 0; i < len; i++) {
if (buf_writeSCID_1(&s->TX, data[i] & 0x00FF) != 0) break;
}
s->sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
s->sci->SCIFFTX.bit.TXFFIENA = 1; // TX: enable fifo interrupt
}
return i;
}
/***************************************************************************
* TXxINT_DATA_SENT()
*
* Executed when transmission is ready for additional data. These functions
* read the next char of data and put it in the TXBUF register for transfer.
***************************************************************************/
#ifdef _FLASH
#pragma CODE_SECTION(TXAINT_data_sent, ".TI.ramfunc");
#endif
__interrupt void TXAINT_data_sent(void)
{
char data;
if (buf_readSCIA_1(&SerialA.TX,0,&data) == 0) {
while ( (buf_readSCIA_1(&SerialA.TX,0,&data) == 0)
&& (SerialA.sci->SCIFFTX.bit.TXFFST != 0x10) ) {
buf_removeSCIA(&SerialA.TX, 1);
SerialA.sci->SCITXBUF.all = data;
}
} else {
SerialA.sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
}
SerialA.sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP9;
}
//for serialB
#ifdef _FLASH
#pragma CODE_SECTION(TXBINT_data_sent, ".TI.ramfunc");
#endif
__interrupt void TXBINT_data_sent(void)
{
char data;
if (buf_readSCIB_1(&SerialB.TX,0,&data) == 0) {
while ( (buf_readSCIB_1(&SerialB.TX,0,&data) == 0)
&& (SerialB.sci->SCIFFTX.bit.TXFFST != 0x10) ) {
buf_removeSCIB(&SerialB.TX, 1);
SerialB.sci->SCITXBUF.all = data;
}
} else {
SerialB.sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
}
SerialB.sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP9;
}
//for serialC
#ifdef _FLASH
#pragma CODE_SECTION(TXCINT_data_sent, ".TI.ramfunc");
#endif
__interrupt void TXCINT_data_sent(void)
{
char data;
if (buf_readSCIC_1(&SerialC.TX,0,&data) == 0) {
while ( (buf_readSCIC_1(&SerialC.TX,0,&data) == 0)
&& (SerialC.sci->SCIFFTX.bit.TXFFST != 0x10) ) {
buf_removeSCIC(&SerialC.TX, 1);
SerialC.sci->SCITXBUF.all = data;
}
} else {
SerialC.sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
}
SerialC.sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
}
//for serialD
#ifdef _FLASH
#pragma CODE_SECTION(TXDINT_data_sent, ".TI.ramfunc");
#endif
__interrupt void TXDINT_data_sent(void)
{
char data;
if (buf_readSCID_1(&SerialD.TX,0,&data) == 0) {
while ( (buf_readSCID_1(&SerialD.TX,0,&data) == 0)
&& (SerialD.sci->SCIFFTX.bit.TXFFST != 0x10) ) {
buf_removeSCID(&SerialD.TX, 1);
SerialD.sci->SCITXBUF.all = data;
}
} else {
SerialD.sci->SCIFFTX.bit.TXFFIENA = 0; // TX: disable fifo interrupt
}
SerialD.sci->SCIFFTX.bit.TXFFINTCLR = 1; // TX: clear interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
}
//extern float turn;
//extern float globalx;
//extern float globaly;
//extern float bearing;
//extern float Vref;
//This function is called each time a char is received over UARTA.
//for SerialA
#ifdef _FLASH
#pragma CODE_SECTION(RXAINT_recv_ready, ".TI.ramfunc");
#endif
__interrupt void RXAINT_recv_ready(void)
{
RXAdata = SciaRegs.SCIRXBUF.all;
/* SCI PE or FE error */
if (RXAdata & 0xC000) {
SciaRegs.SCICTL1.bit.SWRESET = 0;
SciaRegs.SCICTL1.bit.SWRESET = 1;
SciaRegs.SCIFFRX.bit.RXFIFORESET = 0;
SciaRegs.SCIFFRX.bit.RXFIFORESET = 1;
} else {
RXAdata = RXAdata & 0x00FF;
numRXA ++;
if (RXAdata == 'a') {
//turn = turn + 0.05;
} else if (RXAdata == 'd') {
// turn = turn - 0.05;
} else if (RXAdata == 'w') {
// Vref = Vref + 0.1;
} else if (RXAdata == 's') {
// Vref = Vref - 0.1;
} else if (RXAdata == ' ') {
// Vref = 0;
// turn = 0;
} else if (RXAdata == 'm') {
// globalx = 0.0;
// globaly = 0.0;
//EQep1Regs.QPOSCNT = 0;
//EQep2Regs.QPOSCNT = 0;
//bearing = 0.0;
} else {
// turn = 0;
// Vref = 0.5;
}
}
SciaRegs.SCIFFRX.bit.RXFFINTCLR = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP9;
}
//for SerialB
#ifdef _FLASH
#pragma CODE_SECTION(RXBINT_recv_ready, ".TI.ramfunc");
#endif
__interrupt void RXBINT_recv_ready(void)
{
RXBdata = ScibRegs.SCIRXBUF.all;
/* SCI PE or FE error */
if (RXBdata & 0xC000) {
ScibRegs.SCICTL1.bit.SWRESET = 0;
ScibRegs.SCICTL1.bit.SWRESET = 1;
ScibRegs.SCIFFRX.bit.RXFIFORESET = 0;
ScibRegs.SCIFFRX.bit.RXFIFORESET = 1;
} else {
RXBdata = RXBdata & 0x00FF;
// Do something with recieved character
numRXB ++;
}
ScibRegs.SCIFFRX.bit.RXFFINTCLR = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP9;
}
// for SerialC
#ifdef _FLASH
#pragma CODE_SECTION(RXCINT_recv_ready, ".TI.ramfunc");
#endif
__interrupt void RXCINT_recv_ready(void)
{
RXCdata = ScicRegs.SCIRXBUF.all;
/* SCI PE or FE error */
if (RXCdata & 0xC000) {
ScicRegs.SCICTL1.bit.SWRESET = 0;
ScicRegs.SCICTL1.bit.SWRESET = 1;
ScicRegs.SCIFFRX.bit.RXFIFORESET = 0;
ScicRegs.SCIFFRX.bit.RXFIFORESET = 1;
} else {
RXCdata = RXCdata & 0x00FF;
numRXC ++;
}
ScicRegs.SCIFFRX.bit.RXFFINTCLR = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
}
//for SerialD
#ifdef _FLASH
#pragma CODE_SECTION(RXDINT_recv_ready, ".TI.ramfunc");
#endif
int setup = 1;
int i = 0;
__interrupt void RXDINT_recv_ready(void)
{
RXDdata = ScidRegs.SCIRXBUF.all;
/* SCI PE or FE error */
if (RXDdata & 0xC000) {
ScidRegs.SCICTL1.bit.SWRESET = 0;
ScidRegs.SCICTL1.bit.SWRESET = 1;
ScidRegs.SCIFFRX.bit.RXFIFORESET = 0;
ScidRegs.SCIFFRX.bit.RXFIFORESET = 1;
} else {
RXDdata = RXDdata & 0x00FF;
numRXD ++;
if (com_state == 0) { //wait for April Tag data from pi
if (RXDdata == '*'){
com_state = 1;//pre echo state
} else if (RXDdata == '!'){
com_state = 2;//pre sending state
} else if (RXDdata == '#'){
com_state = 3;//wait for desired position data from pi
} else {
com_state = 0;
}
} else if (com_state == 1) {
if (RXDdata == '*') {
com_state = 10;//echo state
} else {
com_state = 0;
}
} else if (com_state == 10) { //echo state
if (received_count % 2 == 0) {
temp_MSB = RXDdata;
} else {
received_pi[received_count/4].parts[(received_count%4)/2] = ((RXDdata << 8) | temp_MSB) & 0xFFFF;
}
received_count += 1;
if (received_count == 12){ // after receiving 3 sets of 4 bytes of data
com_state = 0;
received_count = 0;
cx = received_pi[0].value; //set cx to first 4 bytes
cy = received_pi[1].value; //set cy to second 4 bytes
cbearing = received_pi[2].value; //set cbearing to third 4 bytes
apriltag = 1; //flag that we received data from AprilTags
}
} else if (com_state == 3) {
if (RXDdata == '#') {
com_state = 20;//echo state
} else {
com_state = 0;
}
} else if (com_state == 20) {
if (received_count % 2 == 0) {
temp_MSB = RXDdata;
} else {
received_pi[received_count/4].parts[(received_count%4)/2] = ((RXDdata << 8) | temp_MSB) & 0xFFFF;
}
received_count += 1;
if (received_count == 200){ // after receiving 50 sets of 4 bytes of data
com_state = 0;
received_count = 0;
for (int i = 0;i<25;i++) { //add values to an xarray and a yarray, alternating as their sent one at a time
xarray[i] = received_pi[i*2].value;
yarray[i] = received_pi[i*2+1].value;
}
}
} else if (com_state == 2) {
if (RXDdata == '!') {
int i = 0;
for (i = 0; i < 2; i++) {
to_send[i].value = 0.1234+0.1*i;
}
for (i = 0; i < 2; i++) {
send_sci[4*i] = to_send[i].parts[0] & 0xFF;
send_sci[4*i+1] = (to_send[i].parts[0]>>8) & 0xFF;
send_sci[4*i+2] = to_send[i].parts[1] & 0xFF;
send_sci[4*i+3] = (to_send[i].parts[1]>>8) & 0xFF;
}
serial_sendSCID(&SerialD, send_sci, 8);
com_state = 0;
} else {
com_state = 0;
}
}
}
ScidRegs.SCIFFRX.bit.RXFFINTCLR = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP8;
}
// SerialA only setup for Tera Term connection
char serial_printf_bufSCIA[BUF_SIZESCIA];
uint16_t serial_printf(serialSCIA_t *s, char *fmt, ...)
{
va_list ap;
va_start(ap,fmt);
vsprintf(serial_printf_bufSCIA,fmt,ap);
va_end(ap);
return serial_sendSCIA(s,serial_printf_bufSCIA,strlen(serial_printf_bufSCIA));
}
#!/usr/bin/env python3
'''
Main script. Will use Apriltags to localise position and send via mavlink
'''
import time
from importlib import import_module
from statistics import mean
from collections import deque
import argparse
import threading
import signal
import sys
import os
import socket #for open TCP
import yaml
import numpy
import cv2
from dt_apriltags import Detector
from pymavlink import mavutil
from lib.geo import tagDB
from lib.videoStream import videoThread
from lib.saveStream import saveThread
import serial
import struct
import time
from serial import Serial
exit_event = threading.Event()
#OPEN TCP
TCP_IP = '172.16.112.87'
TCP_PORT = 12321 #can modify if port is busy
BUFFER_SIZE = 1024
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((TCP_IP, TCP_PORT))
s.listen(1)
conn, addr = s.accept()
read_bool = 1;
while read_bool:
data = conn.recv(BUFFER_SIZE)
if not data: break
print("received data:", data) #to verify string is read
read_bool = 0
conn.close()
data = data.decode() #removes b from string
xandy = data.split() #split sent string into array
x = xandy[:len(xandy)//2] #split first half to get x values
y = xandy[len(xandy)//2:] #split second half to get y values
x = [eval(i) for i in x] #convert strings to integers
y = [eval(i) for i in y] #convert strings to integers
print(x)
print(y)
#initialize serial port so we can send drawing data from the LabVIEW and pose data from the AprilTag code to the red board
ser = serial.Serial("/dev/ttyAMA1", 115200) #Open port with baud rate
ser.write(str.encode('#')) #send two hashtags so that C code knows we're sending desired position
ser.write(str.encode('#'))
for i in range(25):
ser.write(struct.pack('f',float(x[i]))) #send floats to C code
ser.write(struct.pack('f',float(y[i])))
print(i) #make sure it finished
time.sleep(.1)
#end drawing data
def signal_handler(signum, frame):
exit_event.set()
#Main class for AprilTag, initialized pose variables, update pose, and send data to the red board
class statusThread(threading.Thread):
#initialized x,y,z postions and row, pitch, yaw angles
def __init__(self):
threading.Thread.__init__(self)
self.lastFiveProTimes = deque(maxlen=5)
self.pos = (0, 0, 0)
self.rot = (0, 0, 0)
self.pktSent = 0
#updates position and angle data to newPos and newRot
def updateData(self, proTime, newPos, newRot, pktWasSent):
self.lastFiveProTimes.append(proTime)
self.pos = newPos
self.rot = newRot
self.pktSent = pktWasSent
#prints postion and angle data to terminal on the Raspberry Pi, sends position and angle data to the red board
def run(self):
while True:
if len(self.lastFiveProTimes) > 0:
fps = 1/mean(self.lastFiveProTimes)
else:
fps = 0
#print out position and angle data in terminal
print("Status: {0:.1f}fps, PosNED = {1}, PosRPY = {2}, Packets sent = {3}".format(
fps, self.pos, self.rot, self.pktSent))
#for j in range(200):
#write data to serial port, this is the protocal for UART
ser.write(str.encode('*'))
ser.write(str.encode('*'))
# we will only send x,y,and bearing (yaw) data to the red board
for i in range(2):
ser.write(struct.pack('f',self.pos[0]))
ser.write(struct.pack('f',self.pos[1]))
ser.write(struct.pack('f',self.rot[2]))
time.sleep(.1)
if exit_event.wait(timeout=2):
return
# class for MAVlink communication protocol, will not be used in our application
class mavThread(threading.Thread):
def __init__(self, device, baud, source_system):
threading.Thread.__init__(self)
self.device = device
self.baud = baud
self.source_system = source_system
self.heartbeatTimestamp = time.time()
self.lock = threading.Lock()
self.conn = None
self.goodToSend = False
self.reset_counter = 0
self.pos = (0, 0, 0)
self.speed = (0, 0, 0)
self.rot = (0, 0, 0)
self.time = 0
self.pktSent = 0
self.target_system = 1
self.origin_lat = -35.363261
self.origin_lon = 149.165230
self.origin_alt = 0
self.posDelta = (0, 0, 0)
self.rotDelta = (0, 0, 0)
def updateData(self, newPos, newRot, t, posDelta, rotDelta):
with self.lock:
if self.time != 0:
# time is in usec here, remember to convert to sec
self.speed = numpy.array(posDelta)/ (1E-6 * (t - self.time))
self.pos = newPos
self.rot = newRot
self.time = t
self.posDelta = posDelta
self.rotDelta = rotDelta
def run(self):
# Start mavlink connection
try:
self.conn = mavutil.mavlink_connection(self.device, autoreconnect=True, source_system=self.source_system,
baud=self.baud, force_connected=False, source_component=mavutil.mavlink.MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY)
except Exception as msg:
print("Failed to start mavlink connection on %s: %s" %
(self.device, msg,))
raise
# wait for the heartbeat msg to find the system ID. Need to exit from here too
# We are sending a heartbeat signal too, to allow ardupilot to init the comms channel
while True:
self.sendHeartbeatAndEKFOrigin()
if self.conn.wait_heartbeat(timeout=0.5) != None:
# Got a hearbeart, go to next loop
self.goodToSend = True
break
if exit_event.is_set():
return
print("Got Heartbeat from APM (system %u component %u)" %
(self.conn.target_system, self.conn.target_system))
self.send_msg_to_gcs("Starting")
while True:
msg = self.conn.recv_match(blocking=True, timeout=0.5)
# loop at 20 Hz
time.sleep(0.05)
self.sendPos()
self.sendSpeed()
#self.sendPosDelta()
self.sendHeartbeatAndEKFOrigin()
if exit_event.is_set():
self.send_msg_to_gcs("Stopping")
return
def sendHeartbeatAndEKFOrigin(self):
# send heartbeat and EKF origin messages if more than 1 sec since last message
if (self.heartbeatTimestamp + 1) < time.time():
self.conn.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0,
0,
0)
self.set_default_global_origin()
self.heartbeatTimestamp = time.time()
def getPktSent(self):
with self.lock:
return self.pktSent
# https://mavlink.io/en/messages/common.html#STATUSTEXT
def send_msg_to_gcs(self, text_to_be_sent):
# MAV_SEVERITY: 0=EMERGENCY 1=ALERT 2=CRITICAL 3=ERROR, 4=WARNING, 5=NOTICE, 6=INFO, 7=DEBUG, 8=ENUM_END
text_msg = 'AprilMAV: ' + text_to_be_sent
self.conn.mav.statustext_send(
mavutil.mavlink.MAV_SEVERITY_INFO, text_msg.encode())
# Send a mavlink SET_GPS_GLOBAL_ORIGIN message (http://mavlink.org/messages/common#SET_GPS_GLOBAL_ORIGIN), for the EKF origin
def set_default_global_origin(self):
current_time_us = int(round(time.time() * 1000000))
self.conn.mav.set_gps_global_origin_send(self.target_system,
int(self.origin_lat*1.0e7),
int(self.origin_lon*1.0e7),
int(self.origin_alt*1.0e3),
current_time_us)
def sendPos(self):
# Send a vision pos estimate
# https://mavlink.io/en/messages/common.html#VISION_POSITION_ESTIMATE
# if self.getTimestamp() > 0:
if self.goodToSend:
current_time_us = int(round(time.time() * 1000000))
# estimate error - approx 0.01m in pos and 0.5deg in angle
cov_pose = 0.01
cov_twist = 0.5
covariance = numpy.array([cov_pose, 0, 0, 0, 0, 0,
cov_pose, 0, 0, 0, 0,
cov_pose, 0, 0, 0,
cov_twist, 0, 0,
cov_twist, 0,
cov_twist])
with self.lock:
self.conn.mav.vision_position_estimate_send(
current_time_us, self.pos[0], self.pos[1], self.pos[2], self.rot[0], self.rot[1], self.rot[2], covariance, reset_counter=self.reset_counter)
self.pktSent += 1
def sendSpeed(self):
# Send a vision speed estimate
# https://mavlink.io/en/messages/common.html#VISION_SPEED_ESTIMATE
if self.goodToSend:
current_time_us = int(round(time.time() * 1000000))
# estimate error - approx 0.05m/s in pos
cov_pose = 0.05
covariance = numpy.array([cov_pose, 0, 0,
0, cov_pose, 0,
0, 0, cov_pose])
with self.lock:
self.conn.mav.vision_speed_estimate_send(
current_time_us, self.speed[0], self.speed[1], self.speed[2], covariance, reset_counter=self.reset_counter)
self.pktSent += 1
def sendPosDelta(self):
# Send a vision pos delta
# https://mavlink.io/en/messages/ardupilotmega.html#VISION_POSITION_DELTA
if self.goodToSend:
with self.lock:
current_time_us = int(round(time.time() * 1000000))
delta_time_us = current_time_us - self.time
current_confidence_level = 80
# Send the message
self.conn.mav.vision_position_delta_send(
current_time_us, # us: Timestamp (UNIX time or time since system boot)
delta_time_us, # us: Time since last reported camera frame
self.rotDelta, # float[3] in radian: Defines a rotation vector in body frame that rotates the vehicle from the previous to the current orientation
self.posDelta, # float[3] in m: Change in position from previous to current frame rotated into body frame (0=forward, 1=right, 2=down)
current_confidence_level # Normalized confidence value from 0 to 100.
)
self.pktSent += 1
#python argument for AprilTags, we will modify the "tagsize" to 96mm (size of our printed tags), "camera" to GenericUSB camera, and "video" to enable camera output
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("--tagSize", type=int, default=96,
help="Apriltag size in mm")
parser.add_argument("--camera", type=str, default="GenericUSB",
help="Camera profile in camera.yaml")
parser.add_argument("--maxerror", type=int, default=400,
help="Maximum pose error to use, in n*E-8 units")
parser.add_argument("--outfile", type=str, default="geo_test_results.csv",
help="Output tag data to this file")
parser.add_argument(
"--device", type=str, default="udpin:127.0.0.1:14550", help="MAVLink connection string")
parser.add_argument("--baud", type=int, default=115200,
help="MAVLink baud rate, if using serial")
parser.add_argument("--source-system", type=int,
default=1, help="MAVLink Source system")
parser.add_argument("--imageFolder", type=str, default="",
help="Save processed images to this folder")
parser.add_argument("--video", type=int, default=1,
help="Output video to port, 0 to disable")
parser.add_argument("--decimation", type=int,
default=2, help="Apriltag decimation")
args = parser.parse_args()
print("Initialising")
# Open camera settings/
with open('camera.yaml', 'r') as stream:
parameters = yaml.load(stream, Loader=yaml.FullLoader)
camParams = parameters[args.camera]
# initialize the camera
camera = None
try:
print(parameters[args.camera]['cam_name'])
mod = import_module("lib." + parameters[args.camera]['cam_name'])
camera = mod.camera(parameters[args.camera])
except (ImportError, KeyError):
print('No camera with the name {0}, exiting'.format(args.camera))
sys.exit(0)
# allow the camera to warmup
time.sleep(2)
at_detector = Detector(searchpath=['apriltags3py/apriltags/lib', 'apriltags3py/apriltags/lib'],
families='tagStandard41h12',
nthreads=4,
quad_decimate=args.decimation,
quad_sigma=0.4,
refine_edges=1,
decode_sharpening=1,
debug=0)
# All tags live in here
tagPlacement = tagDB(False)
outfile = open(args.outfile, "w+")
# left, up, fwd, pitch, yaw, roll
outfile.write("{0},{1},{2},{3},{4},{5},{6}\n".format(
"Filename", "PosX (m)", "PosY (m)", "PosZ (m)", "RotX (rad)", "RotY (rad)", "RotZ (rad)"))
# Need to reconstruct K and D if using fisheye lens (not used in this project)
if camParams['fisheye']:
K = numpy.zeros((3, 3))
D = numpy.zeros((4, 1))
K[0, 0] = camParams['cam_params'][0]
K[1, 1] = camParams['cam_params'][1]
K[0, 2] = camParams['cam_params'][2]
K[1, 2] = camParams['cam_params'][3]
K[2, 2] = 1
D[0][0] = camParams['cam_paramsD'][0]
D[1][0] = camParams['cam_paramsD'][1]
D[2][0] = camParams['cam_paramsD'][2]
D[3][0] = camParams['cam_paramsD'][3]
dim1 = None
signal.signal(signal.SIGINT, signal_handler)
# Start MAVLink comms thread
threadMavlink = mavThread(args.device, args.baud, args.source_system)
threadMavlink.start()
# Start Status thread
threadStatus = statusThread()
threadStatus.start()
# Start save image thread, if desired
threadSave = None
if args.imageFolder != "":
threadSave = saveThread(args.imageFolder, exit_event)
threadSave.start()
# video stream out, if desired
threadVideo = None
if args.video != 0:
threadVideo = videoThread(args.video, exit_event)
threadVideo.start()
i = 0
while True:
# print("--------------------------------------")
myStart = time.time()
# grab an image (and timestamp in usec) from the camera
# estimate 50usec from timestamp to frame capture on next line
timestamp = int(round(time.time() * 1000000)) + 50
file = camera.getFileName()
#print("Timestamp of capture = {0}".format(timestamp))
imageBW = camera.getImage()
i += 1
# we're out of images
if imageBW is None:
break
# AprilDetect, after accounting for distortion (if fisheye)
if camParams['fisheye'] and dim1 is None:
# Only need to get mapping at first frame
# dim1 is the dimension of input image to un-distort
dim1 = imageBW.shape[:2][::-1]
map1, map2 = cv2.fisheye.initUndistortRectifyMap(
K, D, numpy.eye(3), K, dim1, cv2.CV_16SC2)
if camParams['fisheye']:
imageBW = cv2.remap(
imageBW, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
#tags = at_detector.detect(undistorted_img, True, camParams['cam_params'], args.tagSize/1000)
# else:
tags = at_detector.detect(
imageBW, True, camParams['cam_params'], args.tagSize/1000)
# add any new tags to database, or existing one to duplicates
tagsused = 0
for tag in tags:
if tag.pose_err < args.maxerror*1e-8:
tagsused += 1
tagPlacement.addTag(tag)
tagPlacement.getBestTransform()
if file:
print("File: {0}".format(file))
# get current location and rotation state of vehicle in ArduPilot NED format (rel camera)
(posD, rotD) = tagPlacement.getArduPilotNED()
(posR, rotR) = tagPlacement.getArduPilotNED(radians=True)
(posRDelta, rotRDelta) = tagPlacement.getArduPilotNEDDelta(radians=True)
outfile.write("{0},{1:.3f},{2:.3f},{3:.3f},{4:.1f},{5:.1f},{6:.1f}\n".format(
file, posR[0], posR[1], posR[2], rotR[0], rotR[1], rotR[2]))
#print("Time to capture, detect and localise = {0:.3f} sec, using {2}/{1} tags".format(time.time() - myStart, len(tags), len(tagPlacement.tagDuplicatesT)))
# Create and send MAVLink packet
threadMavlink.updateData(posR, rotR, timestamp, posRDelta, rotRDelta)
#wasSent = threadMavlink.sendPos(posR[0], posR[1], posR[2], rotR[0], rotR[1], rotR[2], timestamp)
# Send to status thread
threadStatus.updateData(time.time(
) - myStart, (posD[0], posD[1], posD[2]), (rotD[0], rotD[1], rotD[2]), threadMavlink.getPktSent())
# Send to save thread
if threadSave:
threadSave.save_queue.put((imageBW, os.path.join(
".", args.imageFolder, "processed_{:04d}.jpg".format(i)), posD, rotD, tags))
# Get ready for next frame
tagPlacement.newFrame()
# Send to video stream, if option
if threadVideo:
threadVideo.frame_queue.put((imageBW, posD, rotD, tags))
if exit_event.is_set():
break
//#############################################################################
// 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"
#include "control.h" // this is what I will put my functions in I guess
#include "MatrixMath.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
extern float cx;
extern float cy;
extern float cbearing;
// 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 SPIB_isr(void); //initialize interrupt function for lab 5
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
//initialize variables for lab 5
int count_up = 1; // "boolean" which indicates whether we're increasing (1) or decreasing (0). Start by increasing for exercise 3
uint16_t PWM_val = 0; //init our pwm
int16_t spivalue1 = 0; //raw spi values
int16_t spivalue2 = 0; // raw spi
int16_t spivalue3 = 0; //raw spi
float spival1_volt = 0.0; //spi values converted to voltage
float spival2_volt = 0.0;// voltage
float spival3_volt = 0.0;//voltage
int16_t dummy= 0; //temp value for storing spirxbus's first val
int16_t accelXraw = 0;// raw IMU data
int16_t accelYraw = 0;// raw IMU data
int16_t accelZraw = 0;// raw IMU data
int16_t gyroXraw = 0; //Raw gyroscope data from IMU
int16_t gyroYraw = 0;//Raw gyroscope data from IMU
int16_t gyroZraw = 0;//Raw gyroscope data from IMU
float accelXreading = 0.0;// Raw accel converted to float
float accelYreading = 0;// Raw accel converted to float
float accelZreading = 0;// Raw accel converted to float
float gyroXreading = 0;// Raw gyro converted to float
float gyroYreading = 0;// Raw gyro converted to float
float gyroZreading = 0;// Raw gyro converted to float
int16_t temp = 0;// temp value
//initialize variables for lab 6
float LeftWheel = 0.0; //left wheel angle
float RightWheel = 0.0; // right wheel angle
float PosLeft_k = 0.0; // total distance the left wheel has traveled
float PosRight_k = 0.0; // total distance the right wheel has traveled
float uLeft = 0.0; // velocity of the left wheel
float uRight = 0.0; // velocity of the right wheel
float PosLeft_k_1 = 0.0; //previous position of the left wheel
float PosRight_k_1 = 0.0; // previous pos of the right wheel
float VLeftK = 0.0; // gain of the left wheel
float VRightK = 0.0; // gain of the right wheel
uint16_t TBPRD_val = 2500; //setting the period of the PWM signal here for exercises 1 2 and 4 - we will not change this but it is nice to have set. Set to 2500 bc of 20kHz (and no divide)
float Kp = .5; //proportional constant for controller
float Ki = 1.0; //interral constant for controller
float Vref = 0.0; // reference speed
float ILeft_k = 0.0;
float IRight_k = 0.0;
float ILeft_k_1 = 0.0;
float IRight_k_1 = 0.0;
float eLeft_k = 0.0;
float eRight_k = 0.0;
float eLeft_k_1 = 0.0;
float eRight_k_1 = 0.0;
//uint16_t satL = 0;
//uint16_t satR = 0;
float Kturn = 3.0;
//float eturn = 0.0;
//float turn = 0.0; // value to set the turn
float globalx = 0.0; // dead reckoning x coordinate in ft
float globaly = 0.0; // dead reckoning y coordinate in ft
float bearing = 0.0; // dead reckoning bearing angle in rad
float bearing_k_1 = 0.0; // added when matt fucked with shit
float LeftWheel_k_1 = 0.0;
float RightWheel_k_1 = 0.0;
float xdot_k_1 = 0.0;
float ydot_k_1 = 0.0;
float xdot = 0.0;
float ydot = 0.0;
float wLeftK = 0.0;
float wRightK = 0.0;
float theta_avg_k = 0.0;
float omega_avg_k = 0.0;
extern int ready;
extern float cur_x_e;
extern float cur_y_e;
extern float cur_bearing_e;
extern float cur_vel_x_e;
extern float cur_vel_y_e;
extern float cur_omega_in_e;
extern int last_line_index;
//extern uint16_t TBPRD_val; //setting the period of the PWM signal here for exercises 1 2 and 4 - we will not change this but it is nice to have set. Set to 2500 bc of 20kHz (and no divide)
float eturn=0;
float turn=0;
float eturn_I=0;
float vel_right=0;
float vel_left=0;
float eturn_I_1=0;
float zero_x=0;
float zero_y=0;
float shift_pos_x=0;
float shift_pos_y=0;
float shift_line_x2=0;
float shift_line_y2=0;
float ux=0;
float uy=0;
float top=0;
float bottom=0;
float proj_x=0;
float proj_y=0;
float comp_x=0;
float comp_y=0;
float slope_line=0;
float slope=0;
float slope_robot_line=0;
float eturn_d=0;
float omega=0;
float eturn_1=0;
//float k_turn_I = .5;// this will need to be tuned to damp the system
//float k_turn_p = .2; // tune for pid;
float eturn_p=0;
float k_turn_d = 0; // tune for the d
int satR=0;
int satL=0;
float finalx_1 = 0.0;
float finaly_1 = 0.0;
float finalbearing_1 = 0.0;
float velocity_x = 0.0;
float velocity_y = 0.0;
float omega_final = 0.0;
// initialize variables for 3D Kalman Filter
float F[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // state transition model
float B[3][2] = {{1,0},{1,0},{0,1}}; // control input model
#define ProcUncert 0.0001
#define CovScalar 10
float Q[3][3] = {{ProcUncert,0,ProcUncert/CovScalar},
{0,ProcUncert,ProcUncert/CovScalar},
{ProcUncert/CovScalar,ProcUncert/CovScalar,ProcUncert}}; // process noise (covariance of encoders and gyro)
float H[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // observation model
#define MeasUncert 1
float R[3][3] = {{MeasUncert,0,MeasUncert/CovScalar},
{0,MeasUncert,MeasUncert/CovScalar},
{MeasUncert/CovScalar,MeasUncert/CovScalar,MeasUncert}}; // measurement noise (covariance of april tags)
float S[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // innovation covariance
float S_inv[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // innovation covariance matrix inverse
float K[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // optimal Kalman gain
float x_pred[3][1] = {{0},{0},{0}}; // predicted state
float P_pred[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // predicted covariance (measure of estimated variance of the state)
float u[2][1] = {{0},{0}}; // control input in terms of velocity and angular velocity
float Bu[3][1] = {{0},{0},{0}}; // matrix multiplication of B and u
float temp_3x3[3][3]; // intermediate storage
float temp_3x1[3][1]; // intermediate storage
float ytilde[3][1]; // difference between predictions
float z[3][1]; // state measurement
float eye3[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // 3x3 identity matrix
float apriltag = 0; // flag for when data is recieved from the april tag system
float finalx = 0; // x coordinate generated from kalman filter in ft
float finaly = 0; // y coordinate generated from kalman filter in ft
float finalbearing = 0; // bearing angle generated from kalman filter in rad
void setupSpib(void); // initialize function for exercise 4
void init_eQEPs(void); // initialize functions for lab 6
float readEncLeft(void);
float readEncRight(void);
//void setEPWM2A(float controleffort);
//void setEPWM2B(float controleffort);
void aim2(float bearing_target, float distance);
int working = 0; // just to see whats happening
float x_2 = 0;
float y_2 = 0;
float xarray[100]; // recall size is 500 by 500 so to scale divide by 500 pixels and mult by size of paper in inches
float yarray[100];
float xarray_i[100];
float yarray_i[100];
int line_index = 0;
float last_line_index_float = 0;
int index_of_last_value = 24;
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
ready = 0;
xarray[0] = 0;
yarray[0] = 0;
last_line_index = index_of_last_value;
gen_line(xarray_i[0],yarray_i[0]);
update_pos(0,0,0,0,0,0);
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 pin
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.SPIB_RX_INT = &SPIB_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);// set to ever 10 ms for exercise 2
//ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 20000);// set to ever 20 ms for exercise 3
ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 1000);// set to ever 1 ms for exercise 4
ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 4000); // run every 4 ms for lab 6 exercise 1
ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 4000); //prints every 50 times, so 5 times a second
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //set EPWM2A instead of GPIO2 EXERCISE 2: robot motors
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //set EPWM2B instead of GPIO3 EXERCISE 2: robot motors
GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1); //set EPWM8A instead of GPIO14 EXERCISE 3: RC motors
GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1); //set EPWM8B instead of GPIO15 EXERCISE 3: RC motors
//copied from lab 3 for lab 6
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //set EPWM2A instead of GPIO2 EXERCISE 2: robot motors
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //set EPWM2B instead of GPIO3 EXERCISE 2: robot motors
GpioCtrlRegs.GPAPUD.bit.GPIO14 = 1; // For EPWM8A
GpioCtrlRegs.GPAPUD.bit.GPIO15 = 1; // For EPWM8B
//init pwm 2A and 2B for robot motor (exercise 2)
EPwm2Regs.TBCTL.bit.CTRMODE = 0; //TBCTL Count up mode
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2; // TBCTL Free Soft emulation mode
EPwm2Regs.TBCTL.bit.PHSEN = 0; // TBCTL disable phase loading
EPwm2Regs.TBCTL.bit.CLKDIV = 0; // TBCTL clock divide by 1
EPwm2Regs.TBCTR = 0; // TBCTR start timer at 0
EPwm2Regs.TBPRD = TBPRD_val; // set period to the value specified by the global variable at the top
EPwm2Regs.CMPA.bit.CMPA = TBPRD_val/2; //start duty cycle at 50%
EPwm2Regs.AQCTLA.bit.CAU = 1; // signal pin clear (low) when TBCTR = CMPA
EPwm2Regs.AQCTLA.bit.ZRO = 2; // signal pin set high when TBCTR = 0
EPwm2Regs.TBPHS.bit.TBPHS = 0; // set TBPHS to 0 (phase offset) to be safe
//init for just PWM 2B
EPwm2Regs.CMPB.bit.CMPB = TBPRD_val/2; //start duty cycle at 50%
EPwm2Regs.AQCTLB.bit.CBU = 1; // signal pin clear (low) when TBCTR = CMPA
EPwm2Regs.AQCTLB.bit.ZRO = 2; // signal pin set high when TBCTR = 0
EPwm8Regs.TBCTL.bit.CTRMODE = 0; //TBCTL Count up mode
EPwm8Regs.TBCTL.bit.FREE_SOFT = 2; // TBCTL Free Soft emulation mode
EPwm8Regs.TBCTL.bit.PHSEN = 0; // TBCTL disable phase loading
EPwm8Regs.TBCTL.bit.CLKDIV = 4; // TBCTL clock divide by 16
EPwm8Regs.TBCTR = 0; // TBCTR start timer at 0
EPwm8Regs.TBPRD = 62500; // set period using 50 Hz and clock divide by 16
EPwm8Regs.CMPA.bit.CMPA = 5000; //start duty cycle at 8% (TBPRD * .08)
EPwm8Regs.AQCTLA.bit.CAU = 1; // signal pin clear (low) when TBCTR = CMPA
EPwm8Regs.AQCTLA.bit.ZRO = 2; // signal pin set high when TBCTR = 0
EPwm8Regs.TBPHS.bit.TBPHS = 0; // set TBPHS to 0 (phase offset) to be safe
//init for just PWM 8B
EPwm8Regs.CMPB.bit.CMPB = 5000; //start duty cycle at 8%
EPwm8Regs.AQCTLB.bit.CBU = 1; // signal pin clear (low) when TBCTR = CMPA
EPwm8Regs.AQCTLB.bit.ZRO = 2; // signal pin set high when TBCTR = 0
init_serialSCIA(&SerialA,115200);
// init_serialSCIB(&SerialB,115200);
// init_serialSCIC(&SerialC,115200);
init_serialSCID(&SerialD,115200);
//BEGIN LAB 5: insert code from lab manual - then cut and paste into below function
setupSpib(); //run function before interrupts are enabled and after init_serialSCIA
//LAB 6:
init_eQEPs();
// 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;
IER |= M_INT6; //for lab 5 exercise 2
// 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;
//for lab 5 exercise 2
// Enable SWI in the PIE: Group 6 interrupt 3
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
// to set the last line index
//PRint line ---------------------------------------------------------------------------------------------------------------
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"ADC values 1(should be 0):%.3fV 2: %.3fV 3:%.3fV\r\n",spival1_volt, spival2_volt, spival3_volt);
//serial_printf(&SerialA,"accelx:%.3f accely:%.3f accelz:%.3f gyrox:%.3f gyroy:%.3f gyroz:%.3f \r\n",accelXreading, accelYreading, accelZreading, gyroXreading, gyroYreading, gyroZreading);
//serial_printf(&SerialA, "Left Wheel (rad):%.3f Right Wheel (rad):%.3f Left Wheel (ft):%.3f Right Wheel (ft):%.3f\r\n", LeftWheel, RightWheel, PosLeft_k, PosRight_k); // for lab 6 exercise 1
//serial_printf(&SerialA, "Left Speed (ft/s):%.3f Right Speed (ft/s):%.3f \r\n", VLeftK, VRightK); // for lab 6 exercise 2
//serial_printf(&SerialA, "globax x coord: %.3f global y coord: %.3f \r\n", globalx, globaly);//lab 6 excersize 5
//serial_printf(&SerialA, "cx = %.3f cy = %.3f\r\n", cx, cy);//lab 6 excersize 5
serial_printf(&SerialA, "system is running");
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%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;
//Clear GPIO9 Low to act as a Slave Select. Right now, just to scope. Later to select DAN28027 chip
//GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;
// GpioDataRegs.GPACLEAR.bit.GPIO9 = 1;//setting gpio9 to be 0 //this is for slave select - recall active low
//SpibRegs.SPIFFRX.bit.RXFFIL = 2; // Issue the SPIB_RX_INT when two values are in the RX FIFO for exercise 2
// SpibRegs.SPIFFRX.bit.RXFFIL = 3; // Issue the SPIB_RX_INT when three values are in the RX FIFO for exercise 3
//lab 5 exercise 3
if (PWM_val >= 3000) { //if PWM_val is equal to 3000 (max)...
count_up = 0; //... start counting down
} else if(PWM_val <= 0) { //if PWM_val is equal to 0...
count_up = 1; // ... start counting up
}
if (count_up){
PWM_val+=10;//increase the PWM_val if we are counting up
} else {
PWM_val-=10; //decrease the PWM_val if we are counting down
}
//SpibRegs.SPITXBUF = 0x00DA; // send initial start command
//SpibRegs.SPITXBUF = PWM_val; // send PWM_val
//SpibRegs.SPITXBUF = PWM_val; // send again
//lab 5 exercise 4
// SpibRegs.SPICCR.bit.SPICHAR = 0xF;
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // pulled low
SpibRegs.SPIFFRX.bit.RXFFIL = 8; // Issue the SPIB_RX_INT when 7 values are in the RX FIFO
SpibRegs.SPITXBUF = ((0x8000) | (0x3A00)); // making the bits 10111010000000 by making the top bit 1 check documentation for exact setup
SpibRegs.SPITXBUF = 0;// setting these to 0 for start
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
/*
while(SpibRegs.SPIFFRX.bit.RXFFST !=8); // wait for the correct number of 16 bit values to be received into the RX FIFO
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Slave Select High
temp = SpibRegs.SPIRXBUF; // read the additional number of garbage receive values off the RX FIFO to clear out the RX FIFO
temp = SpibRegs.SPIRXBUF; //read and discard
temp = SpibRegs.SPIRXBUF;//read and discard
temp = SpibRegs.SPIRXBUF;//read and discard
temp = SpibRegs.SPIRXBUF;//read and discard
temp = SpibRegs.SPIRXBUF;//read and discard
temp = SpibRegs.SPIRXBUF;//read
DELAY_US(10); // Delay 10us to allow time for the MPU-2950 to get ready for next transfer. */
}
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
LeftWheel = readEncLeft();// sets left wheel distance (technically total amt of angle change)
RightWheel = readEncRight(); // sets left wheel distance (technically total amt of angle change)
wLeftK = (LeftWheel- LeftWheel_k_1) / 0.004; // calculates velocity based on time difference and previous position
wRightK = (RightWheel - RightWheel_k_1) / 0.004; // same here but right
PosLeft_k = LeftWheel / 5.128; //converts from rad to ft
PosRight_k = RightWheel / 5.128; // doing it again
bearing = .1946/.56759*(RightWheel - LeftWheel); // this finds the "compass" direction. but it is really just the angle change since beginning
theta_avg_k = .5*(RightWheel + LeftWheel); // finds the average distance of the robot
omega_avg_k = .5*(wRightK + wLeftK); // finds the avg velocity of the robot
xdot = .19460*omega_avg_k*cos(bearing); // finds the x velocity of the robot
ydot = .19460*omega_avg_k*sin(bearing); // finds the y velocity of the robot
globalx = globalx + .004*(xdot+xdot_k_1)/2; // this is global x coordinate with starting pos as 0
globaly = globaly + .004*(ydot+ydot_k_1)/2; // global y coordinate with starting pos as 0
VLeftK = (PosLeft_k - PosLeft_k_1) / 0.004; // calculates velocity of the left wheel based on time difference and previous position
VRightK = (PosRight_k - PosRight_k_1) / 0.004; // calculates the velocity of the right wheel based on time difference and previous position
omega = (bearing - bearing_k_1)/0.004; // calculates the angular velocity of the the bearing angle based on time difference and previous angle
velocity_x = (finalx - finalx_1)/.004;
velocity_y= (finalx - finalx_1)/.004;
omega_final = (finalbearing - finalbearing_1)/.004;
// set these values
cur_x_e = finalx; // sets the x coordinate used in the path generation function to the kalman filter results
cur_y_e = finaly; // sets the y coordinate used in the path generation function to the kalman filter results
cur_bearing_e = finalbearing; // sets the bearing angle used in the path generation function to the kalman filter results
cur_vel_x_e = xdot; // sets the x velocity used in the path generation function to the kalman filter results
cur_vel_y_e = ydot; // sets the y velocity used in the path generation function to the kalman filter results
cur_omega_in_e = omega; //sets the angular velocity of the bearing angle used in the path generation function to the kalman filter results
/*
cur_x_e = globalx; //
cur_y_e = globaly; //
cur_bearing_e = bearing; //
cur_vel_x_e = xdot; //
cur_vel_y_e = ydot; //
cur_omega_in_e = omega; //
*/
// Call the path generation function
working = go();
// if(!working & ready) {
// line_index = line_index + 1;
// x_2 = xarray_i[line_index];
// y_2 = yarray_i[line_index];
// gen_line(x_2, y_2); // neex x_2 to be the second point in the line and y_2 to be the first point
//}
PosLeft_k_1 = PosLeft_k;// all of the code in this section is setting the previous values of all of the needed values so we can integrate or derive
PosRight_k_1 = PosRight_k;
eLeft_k_1 = eLeft_k;
eRight_k_1 = eRight_k;
//ILeft_k_1 = ILeft_k;
//IRight_k_1 = IRight_k;
LeftWheel_k_1 = LeftWheel;
RightWheel_k_1 = RightWheel;
xdot_k_1 = xdot;
ydot_k_1 = ydot;
bearing_k_1 = bearing; // was addded when matt fucked with shit
finalbearing_1 = finalbearing;
finalx_1 = finalx;
finaly_1 = finaly;
CpuTimer1.InterruptCount++;
/*
CpuTimer1.InterruptCount++;
PosLeft_k_1 = PosLeft_k;// all of the code in this section is setting the previous values of all of the needed values so we can integrate or derive
PosRight_k_1 = PosRight_k;
eLeft_k_1 = eLeft_k;
eRight_k_1 = eRight_k;
ILeft_k_1 = ILeft_k;
IRight_k_1 = IRight_k;
LeftWheel_k_1 = LeftWheel;
RightWheel_k_1 = RightWheel;
xdot_k_1 = xdot;
ydot_k_1 = ydot;
LeftWheel = readEncLeft();// sets left wheel distance (technically total amt of angle change)
RightWheel = readEncRight(); // sets left wheel distance (technically total amt of angle change)
wLeftK = (LeftWheel- LeftWheel_k_1) / 0.004; // calculates velocity based on time difference and previous position
wRightK = (RightWheel - RightWheel_k_1) / 0.004; // same here but right
PosLeft_k = LeftWheel / 5.128; //converts from rad to ft
PosRight_k = RightWheel / 5.128; // doing it again
bearing = .1946/.56759*(RightWheel - LeftWheel); // this finds the "compass" direction. but it is really just the angle change since beginning
theta_avg_k = .5*(RightWheel + LeftWheel); // finds the average distance of the robot
omega_avg_k = .5*(wRightK + wLeftK); // finds the avg velocity of the robot
xdot = .19460*omega_avg_k*cos(bearing); // finds the x velocity of the robot
ydot = .19460*omega_avg_k*sin(bearing); // finds the yvelocity of the robot
globalx = globalx + .004*(xdot+xdot_k_1)/2; // this is global x coordinate with starting pos as 0
globaly = globaly + .004*(ydot+ydot_k_1)/2; // global y coordinate with starting pos as 0
VLeftK = (PosLeft_k - PosLeft_k_1) / 0.004; // calculates velocity based on time difference and previous position
VRightK = (PosRight_k - PosRight_k_1) / 0.004;
eturn = (VLeftK - VRightK) + turn; // error but the turn is to help with turning
eLeft_k = Vref - VLeftK - Kturn*eturn; // error left
eRight_k = Vref - VRightK + Kturn*eturn; // error right (to be used with pid)
if (satR) {
// don't run the else so that we stop summing no more integral windup
} else {
IRight_k = IRight_k_1 + 0.004 * (eRight_k + eRight_k_1) / 2; // integrate for PID
}
if (satL) {
// dont run the else so that we stop summing
} else {
ILeft_k = ILeft_k_1 + 0.004 * (eLeft_k + eLeft_k_1) / 2; // integrate for PID
}
uLeft = Kp * eLeft_k + Ki * ILeft_k; // PI for left wheel
uRight = Kp * eRight_k + Ki * IRight_k; // PI for right wheel
if ((uRight < 10) & (uRight > -10)) { // this is code to make sure that we don't request too much
satR = 0; // reset the satR
}
if ((uLeft < 10) & (uLeft > -10)) {
satL = 0; // reset the satL
}
*/
// setEPWM2A(uRight); // finally we send the speed request
// setEPWM2B(-uLeft);
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
// import and convert the x and y point values from LabView and set the ready flag so that the path function moves the robot car
for(int j =0; j < 100; j++) {
xarray_i[j] = xarray[j]/500*3;
yarray_i[j] = yarray[j]/500*3;
} if (xarray[1] != 0.0 & yarray[1] != 0.0) {
ready = 1;
}
// Blink LaunchPad Blue LED
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
CpuTimer2.InterruptCount++;
if ((CpuTimer2.InterruptCount % 50) == 0) { // print the voltage every 50 times cpu_timer2_isr is run
UARTPrint = 1;
}
}
__interrupt void SPIB_isr(void){
//spivalue1 = SpibRegs.SPIRXBUF; // Read first 16 bit value off RX FIFO. Probably is zero since no chip.
//spivalue2 = SpibRegs.SPIRXBUF; // Read second 16 bit value off RX FIFO. Again probably zero for exercise 2.
//spivalue3 = SpibRegs.SPIRXBUF; // Read third 16 bit value off RX FIFO. Added for exercise 3 (to read from DAN29027)
//GpioDataRegs.GPASET.bit.GPIO9 = 1; // Set GPIO9 high to end Slave Select. Now Scope. Later to deselect DAN28027
// Later when actually communicating with the DAN28027 do something with the data. Now do nothing.
//spival1_volt = spivalue1 / 4095.0 * 3.3; //convert raw value to voltage values, this first one should be 0
//spival2_volt = spivalue2 / 4095.0 * 3.3; //convert raw value to voltage values, this first one should be 0
//spival3_volt = spivalue3 / 4095.0 * 3.3; //convert raw value to voltage values, this first one should be 0
//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
//exercise 4
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //set the bit high as a chip select
dummy = SpibRegs.SPIRXBUF; // temp value for the first thing sent
accelXraw = SpibRegs.SPIRXBUF;// these are signed lol but really just raw data
accelYraw = SpibRegs.SPIRXBUF;
accelZraw = SpibRegs.SPIRXBUF;
accelXreading = accelXraw*4.0/32767.0; //converting to real value
accelYreading = accelYraw*4.0/32767.0;
accelZreading = accelZraw*4.0/32767.0;
dummy = SpibRegs.SPIRXBUF; // temp
gyroXraw = SpibRegs.SPIRXBUF;// these are signed lol but really just raw data
gyroYraw = SpibRegs.SPIRXBUF;
gyroZraw = SpibRegs.SPIRXBUF;
gyroXreading = gyroXraw*250.0/32767.0;//converting to real value
gyroYreading = gyroYraw*250.0/32767.0;
gyroZreading = gyroZraw*250.0/32767.0;
// 3D Kalman Filter Code
// Import April Tag X, Y, Bearing Angle. We'll call them cx, cy, cbearing
// Step 0: update B, u, and Q
// Bu is the integration of the x, y, and bearing angle. When added to the current values they will be the predicted values for the next time step
// x and y positions come from the optical encoder data
// bearing angle is from the z gyro IMU reading
B[0][0] = cosf(bearing)*0.001;
B[1][0] = sinf(bearing)*0.001;
B[2][1] = 0.001;
u[0][0] = 0.5*(VLeftK + VRightK); // average velocity of left and right wheels
u[1][0] = gyroZreading*PI/180; // rad/s
// Step 1: predict the state and estimate covariance
Matrix3x2_Mult(B, u, Bu); // Bu = B*u
Matrix3x1_Add(x_pred, Bu, x_pred, 1.0, 1.0); // x_pred = x_pred(old) + Bu
Matrix3x3_Add(P_pred, Q, P_pred, 1.0, 1.0); // P_pred = P_pred(old) + Q
// Step 2: If data is sent from the april tag system, incorporate it into the Kalman filter values
if (1 == apriltag) {
apriltag = 0;
// enter the april tag data into the z matrix to later compute error between the current predicted values
z[0][0] = cx*3.28084; // converted from meters to ft
z[1][0] = cy*3.28084; // converted from meters to ft
z[2][0] = cbearing;
// Step 2a: calculate the innovation/measurement residual, ytilde
// this is the error between the measured values and the predicted values
Matrix3x1_Add(z, x_pred, ytilde, 1.0, -1.0); // ytilde = z-x_pred
// Step 2b: calculate innovation covariance, S
Matrix3x3_Add(P_pred, R, S, 1.0, 1.0); // S = P_pred + R
// Step 2c: calculate the optimal Kalman gain, K
Matrix3x3_Invert(S, S_inv);
Matrix3x3_Mult(P_pred, S_inv, K); // K = P_pred*(S^-1)
// Step 2d: update the state estimate x_pred = x_pred(old) + K*ytilde
Matrix3x1_Mult(K, ytilde, temp_3x1);
Matrix3x1_Add(x_pred, temp_3x1, x_pred, 1.0, 1.0);
// Step 2e: update the covariance estimate P_pred = P_pred(old)*(I-K)
Matrix3x3_Add(eye3, K, temp_3x3, 1.0, -1.0);
Matrix3x3_Mult(temp_3x3, P_pred, P_pred);
// reset globax, globaly, bearing to reduce drift
globalx = x_pred[0][0];
globaly = x_pred[1][0];
bearing = x_pred[2][0];
} // end of correction step
// set x,y,theta to the updated and corrected Kalman values.
finalx= x_pred[0][0];
finaly = x_pred[1][0];
finalbearing = x_pred[2][0];
SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1; // Clear Overflow flag
SpibRegs.SPIFFRX.bit.RXFFINTCLR=1; // Clear Interrupt flag
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
}
void setupSpib(void) //Call this function in main() somewhere after the DINT; line of code.
{
//Step 1.
// cut and paste here all the SpibRegs initializations you found for part 3.
//Also dont forget to cut and paste the GPIO settings for GPIO9, 63, 64, 65,66 which are also a part of the SPIB setup.
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0); // Set as GPIO9 and used as DAN28027 SS
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO9 an Output Pin
GpioDataRegs.GPASET.bit.GPIO9 = 1; //Initially Set GPIO9/SS High so DAN28027 is not selected
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0); // Set as GPIO66 and used as MPU-9250 SS
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL); // Make GPIO66 an Output Pin
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //Initially Set GPIO66/SS High so MPU-9250 is not selected
GPIO_SetupPinMux(63, GPIO_MUX_CPU1, 15); //Set GPIO63 pin to SPISIMOB
GPIO_SetupPinMux(64, GPIO_MUX_CPU1, 15); //Set GPIO64 pin to SPISOMIB
GPIO_SetupPinMux(65, GPIO_MUX_CPU1, 15); //Set GPIO65 pin to SPICLKB
EALLOW;
GpioCtrlRegs.GPBPUD.bit.GPIO63 = 0; // Enable Pull-ups on SPI PINs Recommended by TI for SPI Pins
GpioCtrlRegs.GPCPUD.bit.GPIO64 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO65 = 0;
GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3; // Set I/O pin to asynchronous mode recommended for SPI
GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3; // Set I/O pin to asynchronous mode recommended for SPI
GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3; // Set I/O pin to asynchronous mode recommended for SPI
EDIS;
// ---------------------------------------------------------------------------
SpibRegs.SPICCR.bit.SPISWRESET = 0; // Put SPI in Reset
SpibRegs.SPICTL.bit.CLK_PHASE = 1; //This happens to be the mode for both the DAN28027 and
SpibRegs.SPICCR.bit.CLKPOLARITY = 0; //The MPU-9250, Mode 01.
SpibRegs.SPICTL.bit.MASTER_SLAVE = 1; // Set to SPI Master
SpibRegs.SPICCR.bit.SPICHAR = 15; // Set to transmit and receive 16 bits each write to SPITXBUF
SpibRegs.SPICTL.bit.TALK = 1; // Enable transmission
SpibRegs.SPIPRI.bit.FREE = 1; // Free run, continue SPI operation
SpibRegs.SPICTL.bit.SPIINTENA = 0; // Disables the SPI interrupt
SpibRegs.SPIBRR.bit.SPI_BIT_RATE = 49; // Set SCLK bit rate to 1 MHz so 1us period. SPI base clock is
// 50MHZ. And this setting divides that base clock to create SCLKs period
SpibRegs.SPISTS.all = 0x0000; // Clear status flags just in case they are set for some reason
SpibRegs.SPIFFTX.bit.SPIRST = 1;// Pull SPI FIFO out of reset, SPI FIFO can resume transmit or receive.
SpibRegs.SPIFFTX.bit.SPIFFENA = 1; // Enable SPI FIFO enhancements
SpibRegs.SPIFFTX.bit.TXFIFO = 0; // Write 0 to reset the FIFO pointer to zero, and hold in reset
SpibRegs.SPIFFTX.bit.TXFFINTCLR = 1; // Write 1 to clear SPIFFTX[TXFFINT] flag just in case it is set
SpibRegs.SPIFFRX.bit.RXFIFORESET = 0; // Write 0 to reset the FIFO pointer to zero, and hold in reset
SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1; // Write 1 to clear SPIFFRX[RXFFOVF] just in case it is set
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1; // Write 1 to clear SPIFFRX[RXFFINT] flag just in case it is set
SpibRegs.SPIFFRX.bit.RXFFIENA = 1; // Enable the RX FIFO Interrupt. RXFFST >= RXFFIL
SpibRegs.SPIFFCT.bit.TXDLY = 16; // Set delay between transmits to 16 spi clocks. Needed by DAN28027 chip
SpibRegs.SPICCR.bit.SPISWRESET = 1; // Pull the SPI out of reset
SpibRegs.SPIFFTX.bit.TXFIFO = 1; // Release transmit FIFO from reset.
SpibRegs.SPIFFRX.bit.RXFIFORESET = 1; // Re-enable receive FIFO operation
SpibRegs.SPICTL.bit.SPIINTENA = 1; // Enables SPI interrupt. !! I dont think this is needed. Need to Test
SpibRegs.SPIFFRX.bit.RXFFIL = 16; //Interrupt Level to 16 words or more received into FIFO causes interrupt. This is just the initial setting for the register. Will be changed below
SpibRegs.SPICCR.bit.SPICHAR = 0xF; // send 16 bits at a time
SpibRegs.SPIFFCT.bit.TXDLY = 0x00; // Make sure the TXdelay in between each transfer to 0.
//-----------------------------------------------------------------------------------------------------------------
//Step 2.
// perform a multiple 16 bit transfer to initialize MPU-9250 registers 0x13,0x14,0x15,0x16
// 0x17, 0x18, 0x19, 0x1A, 0x1B, 0x1C 0x1D, 0x1E, 0x1F. Use only one SS low to high for all these writes
// some code is given, most you have to fill you yourself.
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // Slave Select Low
// Perform the number of needed writes to SPITXBUF to write to all 13 registers. Remember we are sending 16 bit transfers, so two registers at a time after the first 16 bit transfer.
SpibRegs.SPITXBUF = 0x1300; // To address 00x13 write 0x00
SpibRegs.SPITXBUF = 0x0000; // To address 00x14 and address 00x15, write 0x00
SpibRegs.SPITXBUF = 0x0000; // To address 00x16 and address 00x17, write 0x00
SpibRegs.SPITXBUF = 0x0013; // To address 00x18 write 0x00, and address 19 write 0x13
SpibRegs.SPITXBUF = 0x0200; // To address 00x1A write 0x02, and address 1B write 0x00 //by setting 1A to 02 we are selecting the bandwidth of the IMU // by setting 1B to 00 we are activating the DLPF reg
SpibRegs.SPITXBUF = 0x0806; // To address 00x1C write 0x08, and address 1D write 0x06// 1c > 8 sets full scale to 4g of gyro // 1D > 06 activates the DLPF and sets it to 5 hz
SpibRegs.SPITXBUF = 0x0000; // To address 00x1E write 0x00, and address 1F write 0x00
while(SpibRegs.SPIFFRX.bit.RXFFST !=7); // wait for the correct number of 16 bit values to be received into the RX FIFO
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Slave Select High
temp = SpibRegs.SPIRXBUF; // read the additional number of garbage receive values off the RX FIFO to clear out the RX FIFO
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10); // Delay 10us to allow time for the MPU-2950 to get ready for next transfer.
//Step 3.
// perform a multiple 16 bit transfer to initialize MPU-9250 registers 0x23,0x24,0x25,0x26, 0x27, 0x28, 0x29.
// Use only one SS low to high for all these writes, some code is given, most you have to fill you yourself.
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // Slave Select Low
// Perform the number of needed writes to SPITXBUF to write to all 7 registers
SpibRegs.SPITXBUF = 0x2300; // To address 00x23 write 0x00
SpibRegs.SPITXBUF = 0x408C; // To address 00x24 write 0x40 // To address 00x25 write 0x8C
SpibRegs.SPITXBUF = 0x0288; // To address 00x26 write 0x02 // To address 00x27 write 0x88
SpibRegs.SPITXBUF = 0x0C0A; // To address 00x28 write 0x0C // To address 00x29 write 0x0A
while(SpibRegs.SPIFFRX.bit.RXFFST != 4); // wait for the correct number of 16 bit values to be received into the RX FIFO
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Slave Select High
temp = SpibRegs.SPIRXBUF; // read the additional number of garbage receive values off the RX FIFO to clear out the RX FIFO
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10); // Delay 10us to allow time for the MPU-2950 to get ready for next transfer.
//Step 4.
// perform a single 16 bit transfer to initialize MPU-9250 register 0x2A
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; //CS low
SpibRegs.SPITXBUF = 0x2A81; // Write to address 0x2A the value 0x81
// wait for one byte to be received
while(SpibRegs.SPIFFRX.bit.RXFFST !=1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1; //Slave Select High
temp = SpibRegs.SPIRXBUF; //clear
DELAY_US(10);
// The Remainder of this code is given to you and you do not need to make any changes.
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x3800 | 0x0001); // 0x3800 with a 1 at the end
...
This file has been truncated, please download it to see its full contents.
/*
* control.h
*
* Created on: Dec 15, 2022
* Author: mcy2
*/
#include <math.h>
// here are the variables
struct position {
float x;
float y;
float bearing;
float omega;
float velx;
float vely;
float distance_to_pos;
//float dist_to_line;
//int direction_to_line;// boolean left of the robot is positive
// float distance_along_line;
float bearing_wanted;
};
struct line {
//float line_global_heading;
float x1;
float y1;
float x2;
float y2;
float length;
int index_line;
int pen_down_bool;
};
void aim2(float bearing_target, float distance);
float aim(struct position pos, struct line prev_line);// this function will generate the angle that the wheels should be
void gen_line(float x_2, float y_2);// this function will generate the line that I want to follow
//void set_wheel_speeds(float turn_angle);
void set_pen_down(int pen_down);
void update_pos(float cur_x, float cur_y, float cur_bearing, float cur_vel_x, float cur_vel_y, float omega_in);
int go(void);// this position will make the wheels run, it will return 1 when it is "close enough" to the point 0 is running.
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
Comments