E=MC
.
Diff: main.cpp
- Revision:
- 13:f7ec3f026634
- Parent:
- 8:f3e0b4814888
--- a/main.cpp Fri Mar 06 00:28:42 2015 +0000
+++ b/main.cpp Thu Apr 23 23:24:16 2015 +0000
@@ -1,13 +1,75 @@
#include "mbed.h"
+#include "stdlib.h"
+#include <vector>
+//Outputs
+DigitalOut led1(LED1);
+DigitalOut clk(PTD5);
+DigitalOut si(PTD4);
+PwmOut motor1(PTA12);
+PwmOut motor2(PTA4);
+DigitalOut break1(PTC7);
+DigitalOut break2(PTC0);
+PwmOut servo(PTA5);
-DigitalOut myled(LED1);
-PwmOut servo(PTA5);
-PwmOut motor(PTA4);
Serial pc(USBTX, USBRX); // tx, rx
-// encoder setup and variables
+//Inputs
+AnalogIn camData(PTC2);
+
+//Encoder setup and variables
InterruptIn interrupt(PTA13);
+//Line Tracking Variables --------------------------------
+float ADCdata [128];
+float maxAccum;
+float maxCount;
+float approxPos;
+float prevApproxPos;
+int trackWindow = 30;
+int startWindow;
+int endWindow;
+float maxVal;
+int maxLoc;
+
+bool firstTime;
+
+//Brightness accommodater
+
+
+//Data Collection
+bool dataCol = true;
+int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;
+
+//Line Crossing variables
+int prevTrackLoc;
+int spaceThresh = 1;
+int widthThresh = 10;
+bool space;
+
+//Servo turning parameters
+float straight = 0.00155f;
+float hardLeft = 0.0011f;
+float hardRight = 0.0021f;
+//float hardLeft = 0.0010f;
+//float hardRight = 0.00195f;
+
+//Servo Directions
+float currDir;
+float prevDir;
+
+// All linescan data for the period the car is running on. To be printed after a set amount of time
+//std::vector<std::vector<int> > frames;
+const int numData = 500;
+int lineCenters [numData] = {0};
+int times [numData] = {0};
+float pSteering [numData] = {0.0f};
+float pdSteering [numData] = {0.0f};
+int loopCtr = 0;
+
+//End of Line Tracking Variables -------------------------
+
+//Encoder and Motor Driver Variables ---------------------
+
//Intervals used during encoder data collection to measure velocity
int interval1=0;
int interval2=0;
@@ -20,20 +82,27 @@
//Variables used to for velocity control
float avg_speed = 0;
+float last_speed = 0;
+
float stall_check = 0;
float tuning_val = 1;
+int numInterrupts = 0;
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+float pulsewidth = 0.14f;
+int intTimMod = 0;
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+// Hardware periods
+float motorPeriod = .0025;
+float servoPeriod = .0025;
+
Timer t;
Timer servoTimer;
-
-// Servo parameters
-float lastTurnTime = 0.0f;
-bool servoLeft = true;
+Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
//Observed average speeds for each duty cycle
-const float TUNING_CONSTANT_20 = 3.00;
-const float TUNING_CONSTANT_30 = 4.30;
-const float TUNING_CONSTANT_50 = 6.880;
+
const float PI = 3.14159;
const float WHEEL_CIRCUMFERENCE = .05*PI;
@@ -42,209 +111,264 @@
const float TUNE_AMT = 0.1f;
//Parameters specifying sample sizes and delays for small and large average speed samples
-float num_samples_small = 10.0f;
+float num_samples_small = 3.0f;
float delay_small = 0.05f;
float num_samples_large = 100.0f;
float delay_large = 0.1f;
-// Large and small arrays used to get average velocity values
+//Large and small arrays used to get average velocity values
float large_avg_speed_list [100];
float small_avg_speed_list [10];
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-float get_speed(){
- float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
- float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
- return linearSpeed;
+//End of Encoder and Motor Driver Variables ----------------------
+
+//Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+//PIDcontrol -----------------------------------------------------
+int centerSum = 0;
+int prevLineCenter = 0;
+int prevTime = 0;
+int p,i,d = 1;
+float timeDiff = 0.0f;
+float lastTime = 0.0f;
+
+//Function for speeding up KL25Z ADC
+void initADC(void){
+
+ ADC0->CFG1 = ADC0->CFG1 & (
+ ~(
+ 0x80 // LDLPC = 0 ; no low-power mode
+ | 0x60 // ADIV = 1
+ | 0x10 // Sample time short
+ | 0x03 // input clock = BUS CLK
+ )
+ ) ; // clkdiv <= 1
+ ADC0->CFG2 = ADC0->CFG2
+ | 0x03 ; // Logsample Time 11 = 2 extra ADCK
+ ADC0->SC3 = ADC0->SC3
+ & (~(0x03)) ; // hardware avarage off
+}
+
+
+// Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+float PIDcontrol(int lineCenter){
+ //centerSum += lineCenter;
+ float newTime = t.read();
+ timeDiff = newTime - lastTime;
+ lastTime = newTime;
+ float lineDif = ((float)prevLineCenter - (float)lineCenter)/(float) timeDiff;
+ return p * lineCenter + d * lineDif;
+
}
-float get_avg_speed(float num_samples, float delay) {
+int main() {
+
+ //Alter reg values to speed up KL25Z
+ initADC();
- float avg_avg_speed = 0;
+ //Line Tracker Initializations
+ int integrationCounter = 0;
+
+ //Initial values for directions
+ currDir = 0;
+ prevDir = 0;
- for (int c = 0; c < num_samples; c++) {
- if (num_samples == num_samples_small){
- small_avg_speed_list[c] = get_speed();
- } else if (num_samples == num_samples_large){
- large_avg_speed_list[c] = get_speed();
- //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
- }
- wait(delay);
- }
-
- for (int c = 1; c < num_samples; c++) {
- if (num_samples == num_samples_small){
- avg_avg_speed += small_avg_speed_list[c];
- } else if (num_samples == num_samples_large){
- avg_avg_speed += large_avg_speed_list[c];
- //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
+ // Motor Driver Initializations
+ motor1.period(motorPeriod);
+ motor2.period(motorPeriod);
+
+ // Servo Initialization
+ servo.period(servoPeriod);
+ servo.pulsewidth(hardRight);
+ wait(3);
+
+ motor1.pulsewidth(motorPeriod*pulsewidth);
+ motor2.pulsewidth(motorPeriod*pulsewidth);
+ break1 = 0;
+ break2 = 0;
+
+ firstTime = true;
+
+ t.start();
+
+ if(dataCol){
+ loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
+ printTimer.start();
+ }
+
+ while(1) {
+ if(dataCol){
+ //break out of main loop if enough time has passed;
+ //pc.printf("%i", loopCtr);
+ if(loopCtr >= numData){
+ break;
}
}
- return avg_avg_speed/num_samples;
-}
+ if(integrationCounter % 151== 0){
+ /*
+ //Disable interrupts
+ interrupt.fall(NULL);
+ interrupt.rise(NULL);
+ */
+
+ //Send start of integration signal
+ si = 1;
+ clk = 1;
+
+ si = 0;
+ clk = 0;
+
+ //Reset timing counter for integration
+ integrationCounter = 0;
+
+ //Reset line tracking variables
+ maxAccum = 0;
+ maxCount = 0;
+ approxPos = 0;
+
+ space = false;
-void velocity_control(float duty_cyc, float tuning_const) {
-
- avg_speed = get_avg_speed(num_samples_small, delay_small);
-
- if (avg_speed == stall_check) {
- 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 (avg_speed - tuning_const < -1*TUNE_THRESH){
- tuning_val += TUNE_AMT;
- stall_check = avg_speed;
- } else {
- tuning_val = 1;
- stall_check = avg_speed;
+ }
+ else if (integrationCounter > 129){
+ //Start Timer
+ //t.start();
+
+ //Enable interrupts
+ //interrupt.fall(&fallInterrupt);
+ //interrupt.rise(&riseInterrupt);
+
+ if (firstTime){
+
+ 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] <= maxVal && maxVal - ADCdata[c] < 0.007f){
+ maxAccum += c;
+ maxCount++;
+ if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
+ space = true;
+ }
+ prevTrackLoc = c;
+ }
+ }
+
+ firstTime = false;
+ } else {
+
+ startWindow = prevApproxPos - trackWindow;
+ endWindow = prevApproxPos + trackWindow;
+ if (startWindow < 0){
+ startWindow = 0;
+ }
+ if (endWindow > 118){
+ endWindow = 118;
+ }
+ maxVal = ADCdata[10];
+ for (int c = startWindow; c < endWindow; c++) {
+ if (ADCdata[c] > maxVal){
+ maxVal = ADCdata[c];
+ maxLoc = c;
+ }
+ }
+
+ for (int c = startWindow; c < endWindow; c++) {
+ if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){
+ maxAccum += c;
+ maxCount++;
+ if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
+ space = true;
+ }
+ prevTrackLoc = c;
+ }
+ }
+ }
+ /*
+ //Check if we need to alter integration time due to brightness
+ if (maxVal < 0.15f){
+ intTimMod += 10;
+ } else if (maxVal >= 1) {
+ if (intTimMod > 0) {
+ intTimMod -= 10;
+ }
+ }
+ */
+
+ //Line Crossing Checks
+ if (space) {
+ currDir = prevDir;
+ firstTime = true;
+ } else {
+ approxPos = (float)maxAccum/(float)maxCount;
+
+ if(dataCol){
+ if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%6==0){
+ lineCenters[loopCtr] = approxPos;
+ times[loopCtr] = printTimer.read_ms();
+ pSteering[loopCtr] = currDir;
+ pSteering[loopCtr] = PIDcontrol(approxPos);
+ loopCtr++;
+ }
+ }
+
+ currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
+ prevApproxPos = approxPos;
+
+ }
+
+ servo.pulsewidth(currDir);
+
+ //Start Velocity control after requisite number of encoder signals have been collected
+ //if(numInterrupts >= 4){
+ //velocity_control(0.1f, TUNING_CONSTANT_10);
+ //}
+
+ //Save current direction as previous direction
+ prevDir = currDir;
+
+ //Prepare to start collecting more data
+ integrationCounter = 150;
+
+ //Disable interrupts
+ //interrupt.fall(NULL);
+ //interrupt.rise(NULL);x
+
+ //Stop timer
+ //t.stop();
+ }
+ else{
+ clk = 1;
+ wait_us(intTimMod);
+ ADCdata[integrationCounter - 1] = camData;
+ clk = 0;
+ }
+
+ //clk = 0;
+ integrationCounter++;
+ if(dataCol){
+ loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
+ }
+ //camData.
+
}
- motor.pulsewidth(.0025 * duty_cyc * tuning_val);
-
- pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
- wait(.2);
-}
-
-void servo_sweep(){
- for(float p = 0.001; p<0.002; p+=0.0001){
- servo.pulsewidth(p);
- wait(0.5);
+ if (dataCol){
+ //print frame data
+ pc.printf("printing frame data\n\r");
+ //int frameSize = frames.size();
+ //pc.printf("%i",frameSize);
+ pc.printf("[");
+ for(int i=0; i<numData; i++){
+ if(lineCenters > 0){
+ pc.printf("%i %i %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]);
+ }
+ }
+ pc.printf("]\n\r");
}
}
-void fallInterrupt(){
-
- int time = t.read_us();
- interval1 = time - lastchange2;
- interval2 = lastchange1-lastchange3;
- interval3 = lastchange2 - lastchange4;
- avg_interval = (interval1 + interval2 + interval3)/3;
-
- lastchange4 = lastchange3;
- lastchange3 = lastchange2;
- lastchange2 = lastchange1;
- lastchange1 = time;
- //pc.printf("dark to light time : %d\n\r", interval);
- //pc.printf("fall");
-}
-void riseInterrupt(){
- int time = t.read_us();
- interval1 = time - lastchange2;
- interval2 = lastchange1-lastchange3;
- interval3 = lastchange2 - lastchange4;
- avg_interval = (interval1 + interval2 + interval3)/3;
-
- lastchange4 = lastchange3;
- lastchange3 = lastchange2;
- lastchange2 = lastchange1;
- lastchange1 = time;
- //pc.printf("light to dark time : %d\n\r", interval);
- //pc.printf("rise");
-}
-
-int main() {
- servo.period(0.005);
- motor.period(.0025);
- interrupt.fall(&fallInterrupt);
- interrupt.rise(&riseInterrupt);
-
- t.start();
- while(1){
-
- char choice = pc.getc();
- pc.putc(choice);
-
- switch(choice){
- case '0':
- motor.pulsewidth(0.0);
- pc.printf("0% \n\r");
-
- break;
- case '1':
- motor.pulsewidth(.0025);
- pc.printf("100% \n\r");
- wait(.5);
- pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
-
- break;
- case '2':
- motor.pulsewidth(.0025*.2);
- pc.printf("\n\r20% \n\r");
- wait(.5);
- pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small));
-
- while(1){
- velocity_control(0.2f, TUNING_CONSTANT_20);
- if(servoLeft){
- servo.pulsewidth(.001);
- }
- else{
- servo.pulsewidth(.002);
- }
- float turnTime = servoTimer.read();
- if(turnTime - lastTurnTime > 3.0){
- servoLeft = !servoLeft;
- lastTurnTime = turnTime;
- }
- }
-
- //break;
- case '3':
- motor.pulsewidth(.0025*.3);
- pc.printf("\n\r30% \n\r");
- wait(.5);
- pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
-
- while(1){
- velocity_control(0.3f, TUNING_CONSTANT_30);
- }
-
- //break;
- case '5':
- motor.pulsewidth(.0025*.5);
- pc.printf("\n\r50% \n\r");
- wait(.5);
- pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
-
- while(1){
- velocity_control(0.5f, TUNING_CONSTANT_50);
- }
-
- //break;
- case 'a':
- pc.printf("\n\rGet average velocity of which duty cycle?\n\r");
- choice = pc.getc();
- pc.putc(choice);
-
- switch(choice){
-
- case '2':
- motor.pulsewidth(.0025*0.2f);
- pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
- break;
-
- case '3':
- motor.pulsewidth(.0025*0.3f);
- pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
- break;
-
- case '5':
- motor.pulsewidth(.0025*0.5f);
- pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
- break;
-
- default:
- break;
- }
- break;
-
- default:
- motor.pulsewidth(.0025*0);
- pc.printf("default\n\r");
- break;
- }
-
- //servo_sweep();
- }
-}