Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: CURRENT_CONTROL IIR LSM9DS0 MEDIAN_FILTER PID QEI RF24 SENSOR_FUSION mbed
Revision 5:842372be775c, committed 2016-04-28
- Comitter:
- adam_z
- Date:
- Thu Apr 28 09:39:39 2016 +0000
- Parent:
- 4:a2d38818c4e7
- Child:
- 6:5bd08053e95c
- Commit message:
- PD DONE;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Apr 28 09:09:29 2016 +0000
+++ b/main.cpp Thu Apr 28 09:39:39 2016 +0000
@@ -14,12 +14,15 @@
Ticker WIPVTimer;
void WIPVTimerInterrupt();
float saturation(float input, float limit_H, float limit_L);
+void SensorAcquisition(float * data, float samplingTime);
-CURRENT_CONTROL motorCur_L(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
-CURRENT_CONTROL motorCur_R(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
+//MOTOR L == MOTOR 1; MOTOR R = MOTOR 2
+CURRENT_CONTROL motorCur_L(PC_3, D8, A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
+CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);
-QEI wheel_L(A1, A2, NC, 280, 50, Ts, QEI::X4_ENCODING);
-QEI wheel_R(D13, D12, NC, 280, 50, Ts, QEI::X4_ENCODING);
+QEI wheel_L(D13, D12, NC, 280, 50, Ts, QEI::X4_ENCODING);
+QEI wheel_R(A1, A2, NC, 280, 50, Ts, QEI::X4_ENCODING);
+
PID balancingPD(20,0.00,0.0,Ts);
LPF sensorFilter(Ts);
@@ -27,7 +30,7 @@
-void SensorAcquisition(float * data, float samplingTime);
+
int tim_counter = 0;
float tim = 0.0;
@@ -48,8 +51,8 @@
sensor.setGyroOffset(38,-24,-106);
sensor.setAccelOffset(-793,-511,300);
- motorCur_L.SetParams(3.3*8/0.6, 1.187*0.137, 0.04348);
- motorCur_R.SetParams(3.3*8/0.6, 1.050*0.163, 0.04348);
+ motorCur_L.SetParams(3.3*8/0.6, 1.050*0.163, 0.04348);
+ motorCur_R.SetParams(3.3*8/0.6, 1.187*0.137, 0.04348);
WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
while(true) {
@@ -98,7 +101,7 @@
*/
- float data_array[6];
+ float data_array[6];//Gs and deg/s
data_array[0] = sensor.readFloatAccelX();
data_array[1] = sensor.readFloatAccelY();
data_array[2] = sensor.readFloatAccelZ();
@@ -111,12 +114,12 @@
//*****wheel speed calculation*****//
wheel_L.Calculate();
wheel_R.Calculate();
- //*************obtain current values***********//
- //pc.printf("%f\t", current_L_Offset);
- //pc.printf("%f\r\n", current_R_Offset);
+
+
+
balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
- curCmd_L = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
+ curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
//*************current control********//
tim += Ts;
if(tim >= 4*pi/omega)tim = 0.0;
@@ -124,8 +127,9 @@
//curCmd_L = 0.8;
//motorCur_R.SetPWMDuty(0.75);
- motorCur_L.Control(curCmd_L + 0.002*data_array[4], wheel_L.getAngularSpeed());
- motorCur_R.Control(-curCmd_L + 0.002*data_array[4], wheel_R.getAngularSpeed());
+
+ motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
+ motorCur_R.Control(curCmd_R + 0.002*data_array[4], wheel_R.getAngularSpeed());
}
