GDP 4
/
pid-car-example
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: FRDM-TFC mbed CBuffer XBEE mbed_angular_speed motor2 MMA8451Q
Fork of
KL25Z_Camera_Test
by 
GDP 4
Revision 27:627d67e3b9b0, committed 2016-12-15
- Comitter:
- FatCookies
- Date:
- Thu Dec 15 16:39:32 2016 +0000
- Parent:
- 26:f3d770f3eda1
- Child:
- 28:613239f10ba4
- Commit message:
- changed slip detection (no unintended acceleration), refactored code aka finally got rid of thing
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Dec 15 10:05:07 2016 +0000
+++ b/main.cpp Thu Dec 15 16:39:32 2016 +0000
@@ -32,7 +32,7 @@
Timer t2;
//testing timer
-Timer test;
+Timer lapTimer;
int main() {
@@ -64,8 +64,10 @@
// Check if car is at the stop line
handleStartStop();
- // Send the line scan image over serial
- sendImage();
+ #if USE_COMMS
+ // Send the line scan image over serial
+ sendImage();
+ #endif
//Reset image ready flag
@@ -239,33 +241,28 @@
if(pid->integral > 1.0f) {
pid->integral = 1.0f;
}
- if(pid->integral < -1.0f) {
+ if(pid->integral < -1.0f ) {
pid->integral = -1.0f;
}
}
-
inline void PIDController() {
- // update motor measurements
+ // update motor measurements
// Read the angular velocity of both wheels
+
+ prevL = wL;
+ prevR = wR;
+
wL=Get_Speed(Time_L);
wR=Get_Speed(Time_R);
// Check if left wheel is slipping/giving an abnormal reading and ignore reading
if(wL - prevL < 1.2/0.025) {
left_motor_pid.measured_value = wL;
- prevL = wL;
- } else {
- wL = prevL;
}
-
- // Same as above for right
if(wR - prevR < 1.2/0.025) {
right_motor_pid.measured_value = wR;
- prevR = wR;
- } else {
- wR = prevR;
}
@@ -297,14 +294,14 @@
left_motor_pid.output = 1.0f;
}
if(left_motor_pid.output < -1.0f) {
- left_motor_pid.output = 0.f;
+ left_motor_pid.output = 0.0f;
}
if(right_motor_pid.output > 1.0f) {
right_motor_pid.output = 1.0f;
}
if(right_motor_pid.output < -1.0f) {
- right_motor_pid.output = 0.f;
+ right_motor_pid.output = 0.0f;
}
TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
@@ -370,7 +367,7 @@
for(int i = 30; i < 98; i++) {
currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
difference = lastPixel - currentPixel;
- if(abs(difference) > 20 && lastPixel != -1){ //transition seen, increment counter
+ if(abs(difference) > 10 && lastPixel != -1){ //transition seen, increment counter
transitionsSeen++;
i+=5;
}
@@ -384,8 +381,8 @@
sendString("Start/stop seen");
TFC_SetMotorPWM(0.f,0.f);
TFC_HBRIDGE_DISABLE;
+ lapTime();
}
- transitionsSeen = 0;
// slower++;
}
@@ -416,11 +413,11 @@
if(frame_counter % 256 == 0) {
float level = TFC_ReadBatteryVoltage() * 6.25;
pc.putc('J');
- thing.a = level;
- pc.putc(thing.bytes[0]);
- pc.putc(thing.bytes[1]);
- pc.putc(thing.bytes[2]);
- pc.putc(thing.bytes[3]);
+ byte_float_union.a = level;
+ pc.putc(byte_float_union.bytes[0]);
+ pc.putc(byte_float_union.bytes[1]);
+ pc.putc(byte_float_union.bytes[2]);
+ pc.putc(byte_float_union.bytes[3]);
}
}
@@ -473,16 +470,17 @@
pc.putc('B');
- thing.a = wL * WHEEL_RADIUS;//left_motor_pid.output; //
- pc.putc(thing.bytes[0]);
- pc.putc(thing.bytes[1]);
- pc.putc(thing.bytes[2]);
- pc.putc(thing.bytes[3]);
- thing.a = wR * WHEEL_RADIUS; // right_motor_pid.output; //
- pc.putc(thing.bytes[0]);
- pc.putc(thing.bytes[1]);
- pc.putc(thing.bytes[2]);
- pc.putc(thing.bytes[3]);
+ byte_float_union.a = wL * WHEEL_RADIUS;//left_motor_pid.output; //
+ pc.putc(byte_float_union.bytes[0]);
+ pc.putc(byte_float_union.bytes[1]);
+ pc.putc(byte_float_union.bytes[2]);
+ pc.putc(byte_float_union.bytes[3]);
+ byte_float_union.a = wR * WHEEL_RADIUS; // right_motor_pid.output; //
+ pc.putc(byte_float_union.bytes[0]);
+ pc.putc(byte_float_union.bytes[1]);
+ pc.putc(byte_float_union.bytes[2]);
+ pc.putc(byte_float_union.bytes[3]);
+
}
@@ -492,23 +490,23 @@
case 'A':
if(xb.cBuffer->available() >= 12) {
- thing.bytes[0] = xb.cBuffer->read();
- thing.bytes[1] = xb.cBuffer->read();
- thing.bytes[2] = xb.cBuffer->read();
- thing.bytes[3] = xb.cBuffer->read();
- servo_pid.Kp = thing.a;
+ byte_float_union.bytes[0] = xb.cBuffer->read();
+ byte_float_union.bytes[1] = xb.cBuffer->read();
+ byte_float_union.bytes[2] = xb.cBuffer->read();
+ byte_float_union.bytes[3] = xb.cBuffer->read();
+ servo_pid.Kp = byte_float_union.a;
- thing.bytes[0] = xb.cBuffer->read();
- thing.bytes[1] = xb.cBuffer->read();
- thing.bytes[2] = xb.cBuffer->read();
- thing.bytes[3] = xb.cBuffer->read();
- servo_pid.Ki = thing.a;
+ byte_float_union.bytes[0] = xb.cBuffer->read();
+ byte_float_union.bytes[1] = xb.cBuffer->read();
+ byte_float_union.bytes[2] = xb.cBuffer->read();
+ byte_float_union.bytes[3] = xb.cBuffer->read();
+ servo_pid.Ki = byte_float_union.a;
- thing.bytes[0] = xb.cBuffer->read();
- thing.bytes[1] = xb.cBuffer->read();
- thing.bytes[2] = xb.cBuffer->read();
- thing.bytes[3] = xb.cBuffer->read();
- servo_pid.Kd = thing.a;
+ byte_float_union.bytes[0] = xb.cBuffer->read();
+ byte_float_union.bytes[1] = xb.cBuffer->read();
+ byte_float_union.bytes[2] = xb.cBuffer->read();
+ byte_float_union.bytes[3] = xb.cBuffer->read();
+ servo_pid.Kd = byte_float_union.a;
sendString("pid= Kp: %f, Ki: %f, Kd: %f", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
@@ -519,7 +517,7 @@
case 'F':
if(xb.cBuffer->available() >= 1) {
char a = xb.cBuffer->read();
- speed = (a/0.025f)/50.f;
+ speed = a;
sendString("s = %u %f",a, speed);
curr_cmd = 0;
}
@@ -543,7 +541,7 @@
servo_pid.integral = 0;
- test.start();
+ lapTimer.start();
lapNo =0;
} else if (cmd == 'C') {
@@ -599,7 +597,7 @@
int lapTime()
{
// function which sends the lap time back to the telemetry.
- float newTime= test.read();
+ float newTime= lapTimer.read();
lapNo += 1;
float lapTime= newTime-oldTime;
float avgTime= newTime/lapNo;
@@ -614,10 +612,10 @@
void endTest()
{// This runs when the car has stopped, this should give the final elapsed time and othere things. this also stops the timer
- float time= test.read();
+ float time= lapTimer.read();
sendString("Laps done: %d Time elapsed: %f Average time: %f \n\r",lapNo, time,float (time/lapNo));
- test.stop();
+ lapTimer.stop();
}
--- a/main.h Thu Dec 15 10:05:07 2016 +0000
+++ b/main.h Thu Dec 15 16:39:32 2016 +0000
@@ -68,7 +68,7 @@
union {
float a;
unsigned char bytes[4];
-} thing;
+} byte_float_union;
//Some PID variables
pid_instance servo_pid;