Michael Romain
Just trying to update ccode.
Fork of
coolcarsuperfast
by 
E=MC
Diff: main.cpp
- Revision:
- 5:20223464f7aa
- Parent:
- 4:09c68df71785
- Child:
- 6:f1d948d2d6c1
- Child:
- 7:6d5ddcf12cf3
--- a/main.cpp Sat Mar 21 01:40:29 2015 +0000
+++ b/main.cpp Mon Mar 30 20:07:39 2015 +0000
@@ -5,12 +5,14 @@
DigitalOut led1(LED1);
DigitalOut clk(PTD5);
DigitalOut si(PTD4);
-PwmOut motor1(PTA4);
-PwmOut motor2(PTA12);
-DigitalOut break1(PTC12);
-DigitalOut break2(PTC13);
+PwmOut motor1(PTA12);
+PwmOut motor2(PTA4);
+DigitalOut break1(PTC7);
+DigitalOut break2(PTC0);
PwmOut servo(PTA5);
+Serial pc(USBTX, USBRX); // tx, rx
+
//Inputs
AnalogIn camData(PTC2);
@@ -19,19 +21,27 @@
//Line Tracking Variables --------------------------------
float ADCdata [128];
-float minAccum;
-float minCount;
+float maxAccum;
+float maxCount;
float approxPos;
-float minVal;
-int minLoc;
+float maxVal;
+int maxLoc;
+
+//Line Crossing variables
+int prevTrackLoc;
+bool space;
//Servo turning parameters
float straight = 0.00155f;
-float hardLeft = 0.0012f;
+float hardLeft = 0.0010f;
float slightLeft = 0.00145f;
-float hardRight = 0.0020f;
+float hardRight = 0.0021f;
float slightRight = 0.00165f;
+//Servo Directions
+float currDir;
+float prevDir;
+
//End of Line Tracking Variables -------------------------
//Encoder and Motor Driver Variables ---------------------
@@ -48,15 +58,24 @@
//Variables used to for velocity control
float avg_speed = 0;
+float last_speed = 0;
+
float stall_check = 0;
float tuning_val = 1;
+
+float pulsewidth = 0.18f;
+
+// Hardware periods
+float motorPeriod = .0025;
+float servoPeriod = .0025;
+
Timer t;
Timer servoTimer;
//Observed average speeds for each duty cycle
-const float DESIRED_SPEED = 0.5;
-const float TUNING_CONSTANT_10 = 1.10;
+const float DESIRED_SPEED = 1;
+const float TUNING_CONSTANT_10 = 1.90;
const float TUNING_CONSTANT_20 = 3.00;
const float TUNING_CONSTANT_30 = 4.30;
const float TUNING_CONSTANT_50 = 6.880;
@@ -167,22 +186,38 @@
avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
- if (avg_speed == stall_check) {
+ //When determined speed is infinity or 0, set the speed to the last agreeable speed
+ if (avg_speed > 100 || avg_speed == 0){
+ avg_speed = last_speed;
+ }
+
+ pc.printf("\n\r%f", avg_speed);
+ if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
avg_speed = 0;
tuning_val += TUNE_AMT;
} else if((avg_speed - tuning_const) > TUNE_THRESH){
tuning_val -= TUNE_AMT;
stall_check = avg_speed;
- } else if (tuning_const - avg_speed > TUNE_THRESH){
- //tuning_val += TUNE_AMT;
+
+ } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
+ tuning_val += TUNE_AMT;
stall_check = avg_speed;
} else {
//tuning_val = 1;
stall_check = avg_speed;
}
- motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
- motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
+ if (tuning_val < .5){
+ tuning_val = .5;
+ }
+ pc.printf("\n\rTuning Val: %f", tuning_val);
+ motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
+ motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
+
+ if (avg_speed != 0){
+ last_speed = avg_speed;
+ }
+
}
void fallInterrupt(){
@@ -219,20 +254,25 @@
//Line Tracker Initializations
int integrationCounter = 0;
+ //Initial values for directions (meaningless currently)
+ currDir = 0;
+ prevDir = 0;
+
// Motor Driver Initializations
- motor1.period(.0025);
- motor2.period(.0025);
- //interrupt.fall(&fallInterrupt);
- //interrupt.rise(&riseInterrupt);
+ motor1.period(motorPeriod);
+ motor2.period(motorPeriod);
- wait(5);
+ // Servo Initialization
+ servo.period(servoPeriod);
+
+ wait(3);
- motor1.pulsewidth(.0025*.11);
- motor2.pulsewidth(.0025*.11);
+ motor1.pulsewidth(motorPeriod*pulsewidth);
+ motor2.pulsewidth(motorPeriod*pulsewidth);
break1 = 0;
break2 = 0;
- //t.start();
+ t.start();
//wait(5);
@@ -240,9 +280,8 @@
if(integrationCounter % 151== 0){
//Disable interrupts
- //__disable_irq();
- //interrupt.fall(NULL);
- //interrupt.rise(NULL);
+ interrupt.fall(NULL);
+ interrupt.rise(NULL);
//Send start of integration signal
si = 1;
@@ -255,105 +294,93 @@
integrationCounter = 0;
//Reset line tracking variables
- minAccum = 0;
- minCount = 0;
+ maxAccum = 0;
+ maxCount = 0;
approxPos = 0;
+ space = false;
+
+ /* Velocity control junk
//Reset Encoder and Motor Driver Variables
interval1=0;
interval2=0;
interval3=0;
- avg_interval=0;
+ //avg_interval=0;
lastchange1 = 0;
lastchange2 = 0;
lastchange3 = 0;
lastchange4 = 0;
-
- t.reset();
-
+ //stall_check = 0;
+ */
}
else if (integrationCounter > 129){
- //Enable interrupts
- //__enable_irq();
- //interrupt.fall(&fallInterrupt);
- //interrupt.rise(&riseInterrupt);
+ //Start Timer
+ t.start();
- minVal = ADCdata[15];
- for (int c = 10; c < 118; c++) {
- if (ADCdata[c] > minVal){
- minVal = ADCdata[c];
- minLoc = c;
+ //Enable interrupts
+ interrupt.fall(&fallInterrupt);
+ interrupt.rise(&riseInterrupt);
+
+ maxVal = ADCdata[10];
+ for (int c = 11; c < 118; c++) {
+ if (ADCdata[c] > maxVal){
+ maxVal = ADCdata[c];
+ maxLoc = c;
}
}
for (int c = 10; c < 118; c++) {
- if (ADCdata[c] <= minVal && minVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
- minAccum += c;
- minCount++;
+ if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
+ maxAccum += c;
+ maxCount++;
+ if (c != prevTrackLoc + 1){
+ space = true;
+ }
+ prevTrackLoc = c;
}
}
- approxPos = (float)minAccum/(float)minCount;
-
+ if (maxCount >= 25) {
+ currDir = prevDir;
+ } else {
+ approxPos = (float)maxAccum/(float)maxCount;
+ currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
+ }
//approxPos = find_track(ADCdata);
- /* if(approxPos > 0 && approxPos <= 25){
- servo.pulsewidth(hardLeft);
- motor1.pulsewidth(.0025*.1);
- motor2.pulsewidth(.0025*.1);
- } else if (approxPos > 25 && approxPos <= 40){
- servo.pulsewidth(slightLeft);
- motor1.pulsewidth(.0025*.105);
- motor2.pulsewidth(.0025*.105);
- } else if (approxPos > 40 && approxPos <= 90){
- servo.pulsewidth(straight);
- motor1.pulsewidth(.0025*.11);
- motor2.pulsewidth(.0025*.11);
- } else if (approxPos > 90 && approxPos <= 105){
- servo.pulsewidth(slightRight);
- motor1.pulsewidth(.0025*.105);
- motor2.pulsewidth(.0025*.105);
- } else if (approxPos > 105 && approxPos <= 128){
- servo.pulsewidth(hardRight);
- motor1.pulsewidth(.0025*.1);
- motor2.pulsewidth(.0025*.1);
- }
- */
-
/*if(approxPos > 0 && approxPos <= 45){
- servo.pulsewidth(hardLeft);
- motor1.pulsewidth(.0025*.095);
- motor2.pulsewidth(.0025*.095);
+ motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
+ motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
} else if (approxPos > 45 && approxPos <= 95){
- servo.pulsewidth(straight);
- motor1.pulsewidth(.0025*.11);
- motor2.pulsewidth(.0025*.11);
+ motor1.pulsewidth(motorPeriod*pulsewidth);
+ motor2.pulsewidth(motorPeriod*pulsewidth);
} else {
- servo.pulsewidth(hardRight);
- motor1.pulsewidth(.0025*.095);
- motor2.pulsewidth(.0025*.095);
+ motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
+ motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
}*/
- if(approxPos > 0 && approxPos <= 45){
- motor1.pulsewidth(.0025*.095);
- motor2.pulsewidth(.0025*.095);
- } else if (approxPos > 45 && approxPos <= 95){
- motor1.pulsewidth(.0025*.11);
- motor2.pulsewidth(.0025*.11);
- } else {
- motor1.pulsewidth(.0025*.095);
- motor2.pulsewidth(.0025*.095);
- }
- servo.pulsewidth(.0012 + approxPos/((float) 128)*.0008);
+ servo.pulsewidth(currDir);
+
+ //delay for velocity control
+ //wait_ms(10);
+
- //velocity_control(0.05f, DESIRED_SPEED);
+ //Stop Timer
+ //t.stop();
+
+ //Reset Timer
+ //t.reset();
+
+ //velocity_control(0.1f, TUNING_CONSTANT_10);
+
+ prevDir = currDir;
integrationCounter = 150;
}
else{
clk = 1;
- wait_us(80);
+ wait_us(10);
ADCdata[integrationCounter - 1] = camData;
clk = 0;
}