E=MC
/
e=mc
.
main.cpp
- Committer:
- mawk2311
- Date:
- 2015-03-06
- Revision:
- 9:aecffea74d88
- Parent:
- 7:ea395616348c
File content as of revision 9:aecffea74d88:
#include "mbed.h" DigitalOut myled(LED1); PwmOut servo(PTA5); PwmOut motor(PTA4); Serial pc(USBTX, USBRX); // tx, rx // encoder setup and variables InterruptIn interrupt(PTA13); //Intervals used during encoder data collection to measure velocity int interval1=0; int interval2=0; int interval3=0; int avg_interval=0; int lastchange1 = 0; int lastchange2 = 0; int lastchange3 = 0; int lastchange4 = 0; //Variables used to for velocity control float avg_speed = 0; float stall_check = 0; float tuning_val = 1; Timer t; //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; //Velocity Control Tuning Constants const float TUNE_THRESH = 0.5f; 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 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 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; } float get_avg_speed(float num_samples, float delay) { float avg_avg_speed = 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]); } } return avg_avg_speed/num_samples; } 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; } 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); } } 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); wait(5); char choice = '3'; 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); } //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(); } }