USNA WSE ES456
Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of
Madpulse_Speed_Control_temp
by 
USNA WSE ES456
Revision 2:2bb375ab3b2b, committed 2017-11-15
- Comitter:
- jdawkins
- Date:
- Wed Nov 15 21:51:31 2017 +0000
- Parent:
- 1:f4c9926fb4c9
- Commit message:
- Initial Template for Project Part 1
Changed in this revision
| madpulse_ctrl.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/madpulse_ctrl.cpp Wed Nov 15 18:36:30 2017 +0000
+++ b/madpulse_ctrl.cpp Wed Nov 15 21:51:31 2017 +0000
@@ -33,10 +33,14 @@
// controlling the vehicle
MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
-///////////////////////////////////////////////
-// Define Control Gains and Controller Variable
-//////////////////////////////////////////////
+// Control Gains
+float Kp_spd = 0.0;
+float Ki_spd = 0.0;
+
+float Kp_psi = 0.0;
+float Kd_psi = 0.0;
+float L = 0.01;
float psi_est,psi_est_old;
float spd_err,spd_err_i;
@@ -56,21 +60,32 @@
bool rc_conn,auto_flag,log_flag;
+// ============================================
+// Control Loop
+// ============================================
+// Heading & Speed Control Function
+// Sampled at 100 HZ
void controlLoop(){
- dt = t.read()-t_ctrl;
+ dt = t.read()-t_ctrl; // Sample time (sec)
- if(auto_flag){ // If vehicle is in Auto Mode
+ if(auto_flag){
+ // 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)
+ spd = mp.getFreq()*0.0436; // Read odmeter (meters/sec)
+
+ // Steering Command (+/- 1)
+ str = 0.0; // Set steering trim
+
+ // Throttle Command (+/- 1)
+ if(t.read()-t_run < 1){ // Set throttle value for 1 secs
+ thr = 0.05;
}else{
thr = 0.0;
}
+ // Use manual steering for speed control testing
+ str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
}else{
+ // Maunal RC Mode
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
}
@@ -78,27 +93,26 @@
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);
+// ============================================
+// Data Log
+// ============================================
+// Print data to Xbee serial port (115200 Baud)
+void dataLog(){
+ xbee.printf("%.3f,%d,%.3f,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),dist,spd,thr,str);
}
void initializeControl(){
printf("Initialize Control\r\n");
t_run = t.read();
+ dist = 0.0;
+ spd_err_i = 0;
}
void stopControl(){
- printf("Stop Control\r\n");
+ printf("Stop Control\r\n");
+ spd_err_i = 0;
}
-
void readRCSignal(){
if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
rc_conn = false;
@@ -106,26 +120,22 @@
rc_conn = true;
} // end if(Channels connected)
- if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
+ /* if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
auto_flag = 0;
- }
+ }*/
}
void readKeyboard(){
if(xbee.readable()) {
char c = xbee.getc();
if(c == 'H' || c == 'h') {
xbee.printf("Command List...\r\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("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("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;
@@ -134,9 +144,11 @@
if(c == 'S' || c == 's') {
auto_flag = false;
log_flag = false;
- stopControl();
- }
+ }
+ if(c == 'L' || c == 'l') {
+ log_flag = !log_flag;
+ }
}
}
@@ -166,19 +178,20 @@
// 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
- t_5hz = t.read();
+ // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
+ 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();
}
@@ -187,23 +200,27 @@
if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
// Faster loop for logging data for plotting and analysis
- if(log_flag){
- logData();
- }
+
+ mp.getAccel();
+ mp.getGyro();
+ mp.getMag();
+ mp.getEuler();
+ mp.getSpeed();
+ //xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
+
+ if(log_flag){
+ dataLog();
+ }
t_log = t.read();
}
if(rc_conn) {
- // 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
mp.setSteer(str);
mp.setThrottle(thr);
} else {
Steer.write(1500); // Write Steering Pulse
Throttle.write(1500);
- // mp.setSteer(str);
- // mp.setThrottle(thr);
}
wait(1.0/MAIN_LOOP_FREQ);
@@ -232,5 +249,4 @@
ang = ang + 2*PI;
}
return ang;
-}
-
+}
\ No newline at end of file