USNA WSE ES456
Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of
Madpulse_Control_Fall2017
by 
USNA WSE ES456
Revision 1:f4c9926fb4c9, committed 2017-11-15
- Comitter:
- jdawkins
- Date:
- Wed Nov 15 18:36:30 2017 +0000
- Parent:
- 0:daea75c21ac1
- Commit message:
- Committ for class
Changed in this revision
--- a/Madpulse.lib Tue Nov 14 13:51:29 2017 +0000 +++ b/Madpulse.lib Wed Nov 15 18:36:30 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/USNA-WSE-ES456/code/Madpulse/#5e55308aa7a6 +https://os.mbed.com/teams/USNA-WSE-ES456/code/Madpulse/#6bba42f337e6
--- a/NeoStrip.lib Tue Nov 14 13:51:29 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/NeoStrip/#b2687bc7cafe
--- a/madpulse_ctrl.cpp Tue Nov 14 13:51:29 2017 +0000
+++ b/madpulse_ctrl.cpp Wed Nov 15 18:36:30 2017 +0000
@@ -33,14 +33,10 @@
// controlling the vehicle
MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
-// Control Gains
+///////////////////////////////////////////////
+// Define Control Gains and Controller Variable
+//////////////////////////////////////////////
-float Kp_spd = 0.06;
-float Ki_spd = 0.05;
-
-float Kp_psi = 0.65;
-float Kd_psi = 0.1;
-float L = 0.01;
float psi_est,psi_est_old;
float spd_err,spd_err_i;
@@ -57,7 +53,42 @@
float str,thr,wheel_spd;
float wrapTo2pi(float ang);
float wrapTopi(float ang);
-bool rc_conn,auto_flag;
+bool rc_conn,auto_flag,log_flag;
+
+
+void controlLoop(){
+ dt = t.read()-t_ctrl;
+
+ if(auto_flag){ // If vehicle is in Auto Mode
+
+ spd = mp.getFreq()/27.8119; // Convert from counts per second to meters/second
+
+ str = 0.001;
+ if(t.read()-t_run < 1){
+
+ thr = 0.1; // Set throttle, range (-1 to 1)
+ }else{
+ thr = 0.0;
+ }
+ }else{
+ str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
+ thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
+ }
+
+ t_ctrl = t.read();
+}
+
+void logData(){
+// Accel Data: mp.accel.x,mp.accel.y,mp.accel.z
+// Gyro Data: mp.gyro.x,mp.gyro.y,mp.gyro.z
+// Mag Data: mp.mag.x,mp.mag.y,mp.mag.z
+// Euler Data: mp.euler.roll,mp.euler.pitch, mp.euler.yaw
+// Encoder Data: mp.getCounts(), mp.getFreq()
+
+
+ xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
+}
+
void initializeControl(){
printf("Initialize Control\r\n");
@@ -67,6 +98,7 @@
void stopControl(){
printf("Stop Control\r\n");
}
+
void readRCSignal(){
if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
rc_conn = false;
@@ -83,55 +115,31 @@
char c = xbee.getc();
if(c == 'H' || c == 'h') {
xbee.printf("Command List...\r\n");
- xbee.printf("d - set open loop duty cycle\r\n");
- xbee.printf("r - run open loop control with current duty cycle\r\n");
- xbee.printf("h - display this prompt and exit\r\n");
- xbee.printf("t - set duration of test\r\n\n\n");
+ xbee.printf("h - Display this prompt and return\r\n");
+ xbee.printf("l - Toggle Logging\r\n");
+ xbee.printf("r - Run, switch vehicle to auto mode\r\n");
+ xbee.printf("s - Stop, stop vehicle autor control\r\n");
xbee.printf("Enter Command\r\n");
}
+ if(c == 'L' || c == 'l') {
+ log_flag = !log_flag; //toggle logging flag
+
+ }
if(c == 'R' || c == 'r') {
auto_flag = true;
+ log_flag = true;
initializeControl();
}
if(c == 'S' || c == 's') {
auto_flag = false;
- }
+ log_flag = false;
+ stopControl();
+ }
- if(c == 'T' || c == 't') {
-
- }
}
}
-void controlLoop(){
- dt = t.read()-t_ctrl;
- if(auto_flag){ // If vehicle is in Auto Mode
-
- float des_psi = 0;
- spd = mp.getFreq()/27.8119;
- //str = -Kp_psi*wrapTopi(des_psi-wrapTopi(mp.euler.yaw));
- str = 0.001;
- if(t.read()-t_run < 10){
- des_spd = 1.5;
- spd_err = des_spd-spd;
- spd_err_i += spd_err*dt;
- // spd_err_d = (spd - spd_old)/dt;
-
-
- thr = Kp_spd*spd_err + Ki_spd*spd_err_i + 0.04;
-
- // thr = 0.04;
- }else{
- thr = 0.0;
- }
- }else{
- str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
- thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
- }
-
- t_ctrl = t.read();
-}
int main()
@@ -158,20 +166,19 @@
// check for servo pulse from either channel of receiver module
readRCSignal();
-
-
-
-
if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
// 5 Hz loop intended for debugging print statements
- // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
- t_5hz = t.read();
-
+ t_5hz = t.read();
}
if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
// 100 Hz loop for calling control loop
+ mp.getAccel();
+ mp.getGyro();
+ mp.getMag();
+ mp.getEuler();
+ mp.getSpeed();
controlLoop();
t_ctrl = t.read();
}
@@ -180,25 +187,9 @@
if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
// Faster loop for logging data for plotting and analysis
-
- mp.getAccel();
- mp.getGyro();
- // xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", mp.accel.x,mp.accel.y,mp.accel.z,mp.gyro.x,mp.gyro.y,mp.gyro.z);
-
-
- mp.getMag();
- //xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",mp.mag.x,mp.mag.y,mp.mag.z);
-
-
- mp.getEuler();
- // xbee.printf("$RPY,%.3f, %.3f, %.3f\r\n", mp.euler.roll,mp.euler.pitch,wrapTo2pi(mp.euler.yaw));
-
-
- mp.getSpeed();
- xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
-
-
-
+ if(log_flag){
+ logData();
+ }
t_log = t.read();
}