E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@4:263bddc51c0f, 2015-02-27 (annotated)

Committer:
ericoneill
Date:
Fri Feb 27 04:19:03 2015 +0000
Revision:
4:263bddc51c0f
Parent:
3:7eaf505f811e
Child:
5:61a0a21134f7
linear speed calculated, need to do control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 0:d328ecb3fbb1 1 #include "mbed.h"
ericoneill 0:d328ecb3fbb1 2
ericoneill 0:d328ecb3fbb1 3 DigitalOut myled(LED1);
ericoneill 0:d328ecb3fbb1 4 PwmOut servo(PTA5);
ericoneill 0:d328ecb3fbb1 5 PwmOut motor(PTA4);
ericoneill 0:d328ecb3fbb1 6 Serial pc(USBTX, USBRX); // tx, rx
ericoneill 3:7eaf505f811e 7
ericoneill 3:7eaf505f811e 8 // encoder setup and variables
ericoneill 3:7eaf505f811e 9 InterruptIn interrupt(PTA13);
ericoneill 4:263bddc51c0f 10
ericoneill 4:263bddc51c0f 11 int interval1=0;
ericoneill 4:263bddc51c0f 12 int interval2=0;
ericoneill 4:263bddc51c0f 13 int interval3=0;
ericoneill 4:263bddc51c0f 14 int avg_interval=0;
ericoneill 4:263bddc51c0f 15 int lastchange1 = 0;
ericoneill 4:263bddc51c0f 16 int lastchange2 = 0;
ericoneill 4:263bddc51c0f 17 int lastchange3 = 0;
ericoneill 4:263bddc51c0f 18 int lastchange4 = 0;
ericoneill 1:8e5821dec0f7 19 Timer t;
ericoneill 4:263bddc51c0f 20 //constant *(PWM-expected)
ericoneill 4:263bddc51c0f 21 const float TUNING_CONSTANT = .5;
ericoneill 4:263bddc51c0f 22 const float PI = 3.14159;
ericoneill 4:263bddc51c0f 23 const float WHEEL_CIRCUMFERENCE = .05*PI;
ericoneill 0:d328ecb3fbb1 24
ericoneill 4:263bddc51c0f 25 float get_speed(){
ericoneill 4:263bddc51c0f 26 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
ericoneill 4:263bddc51c0f 27 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
ericoneill 4:263bddc51c0f 28 return linearSpeed;
ericoneill 4:263bddc51c0f 29
ericoneill 4:263bddc51c0f 30 }
ericoneill 0:d328ecb3fbb1 31 void servo_sweep(){
ericoneill 0:d328ecb3fbb1 32 for(float p = 0.001; p<0.002; p+=0.0001){
ericoneill 0:d328ecb3fbb1 33 servo.pulsewidth(p);
ericoneill 0:d328ecb3fbb1 34 wait(0.5);
ericoneill 0:d328ecb3fbb1 35 }
ericoneill 0:d328ecb3fbb1 36 }
ericoneill 3:7eaf505f811e 37 void fallInterrupt(){
ericoneill 4:263bddc51c0f 38
ericoneill 4:263bddc51c0f 39 int time = t.read_us();
ericoneill 4:263bddc51c0f 40 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 41 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 42 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 43 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 44
ericoneill 4:263bddc51c0f 45 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 46 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 47 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 48 lastchange1 = time;
ericoneill 4:263bddc51c0f 49 //pc.printf("dark to light time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 50 //pc.printf("fall");
ericoneill 3:7eaf505f811e 51 }
ericoneill 3:7eaf505f811e 52 void riseInterrupt(){
ericoneill 4:263bddc51c0f 53 int time = t.read_us();
ericoneill 4:263bddc51c0f 54 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 55 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 56 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 57 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 58
ericoneill 4:263bddc51c0f 59 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 60 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 61 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 62 lastchange1 = time;
ericoneill 4:263bddc51c0f 63 //pc.printf("light to dark time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 64 //pc.printf("rise");
ericoneill 3:7eaf505f811e 65 }
ericoneill 3:7eaf505f811e 66
ericoneill 0:d328ecb3fbb1 67 int main() {
ericoneill 0:d328ecb3fbb1 68 servo.period(0.005);
ericoneill 0:d328ecb3fbb1 69 motor.period(.0025);
ericoneill 3:7eaf505f811e 70 interrupt.fall(&fallInterrupt);
ericoneill 3:7eaf505f811e 71 interrupt.rise(&riseInterrupt);
ericoneill 3:7eaf505f811e 72
ericoneill 1:8e5821dec0f7 73
ericoneill 1:8e5821dec0f7 74 t.start();
ericoneill 4:263bddc51c0f 75 while(1){
ericoneill 2:30ebae0d3e17 76
ericoneill 2:30ebae0d3e17 77 char choice = pc.getc();
ericoneill 2:30ebae0d3e17 78 pc.putc(choice);
ericoneill 2:30ebae0d3e17 79 switch(choice){
ericoneill 0:d328ecb3fbb1 80 case '0':
ericoneill 0:d328ecb3fbb1 81 motor.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 82 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 83
ericoneill 0:d328ecb3fbb1 84 break;
ericoneill 0:d328ecb3fbb1 85 case '1':
ericoneill 0:d328ecb3fbb1 86 motor.pulsewidth(.0025);
ericoneill 2:30ebae0d3e17 87 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 88 wait(.5);
ericoneill 4:263bddc51c0f 89 pc.printf("speed: %f",get_speed());
ericoneill 4:263bddc51c0f 90 break;
ericoneill 4:263bddc51c0f 91 case '2':
ericoneill 4:263bddc51c0f 92 motor.pulsewidth(.0025*.2);
ericoneill 4:263bddc51c0f 93 pc.printf("20% \n\r");
ericoneill 4:263bddc51c0f 94 wait(.5);
ericoneill 4:263bddc51c0f 95 pc.printf("speed: %f",get_speed());
ericoneill 0:d328ecb3fbb1 96 break;
ericoneill 0:d328ecb3fbb1 97 case '3':
ericoneill 0:d328ecb3fbb1 98 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 99 pc.printf("30% \n\r");
ericoneill 4:263bddc51c0f 100 wait(.5);
ericoneill 4:263bddc51c0f 101 pc.printf("speed: %f",get_speed());
ericoneill 0:d328ecb3fbb1 102 break;
ericoneill 0:d328ecb3fbb1 103 case '5':
ericoneill 0:d328ecb3fbb1 104 motor.pulsewidth(.0025*.5);
ericoneill 2:30ebae0d3e17 105 pc.printf("50% \n\r");
ericoneill 4:263bddc51c0f 106 wait(.5);
ericoneill 4:263bddc51c0f 107 pc.printf("speed: %f",get_speed());
ericoneill 0:d328ecb3fbb1 108 break;
ericoneill 0:d328ecb3fbb1 109 default:
ericoneill 0:d328ecb3fbb1 110 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 111 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 112 break;
ericoneill 0:d328ecb3fbb1 113 }
ericoneill 3:7eaf505f811e 114
ericoneill 0:d328ecb3fbb1 115 //servo_sweep();
ericoneill 0:d328ecb3fbb1 116 }
ericoneill 0:d328ecb3fbb1 117 }