![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
4180 final project slave mbed code
Dependencies: 4DGL-uLCD-SE Motor mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:6d4adda58ec2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Dec 09 15:31:23 2016 +0000 @@ -0,0 +1,502 @@ +#include "mbed.h" +#include "uLCD_4DGL.h" +#include "Motor.h" +#include "rtos.h" +#include "stdio.h" + +//Motors +Motor m_R(p21, p5, p6); // pwm, fwd, rev _ right wheel +Motor m_L(p22, p7, p8); // pwm, fwd, rev _ left wheel + +//Serial communication between 2 mbeds +Serial pc(USBTX, USBRX); +Serial uart(p9, p10); + +// Timer and halleffect sensor interrupts +Timer t; +InterruptIn risingEdge_L(p24); +InterruptIn risingEdge_R(p25); + +// uLCD for testing +uLCD_4DGL uLCD(p28,p27,p29); + +// LEDs for testing +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); + +//global variables for the halleffect sensors +volatile long int count_L; +volatile long int count_nr_L; + +volatile long int count_R; +volatile long int count_nr_R; + +volatile int count; + +volatile char ms[1]; +volatile char i ; + +/* +volatile int speed_h = 6; +volatile int speed_m = 5; +volatile int speed_l = 4; +*/ + + +void pulses_L() +{ + if(myled2 == 1) + { + myled2 = 0; + } + else + { + myled2 = 1; + } + count_L++; + count_nr_L++; +} + +void pulses_R() +{ + if(myled4 == 1) + { + myled4 = 0; + } + else + { + myled4 = 1; + } + count_R++; + count_nr_R++; +} + +int main() { + uart.baud(9600); + + double circumference = 6.5 * 3.1416; // 6 cm wheel diameter * pi + long int temp_L; + long int temp_R; + double sA = 0.52; + double sB = 0.55; + + double dist_L; + double dist_R; + + count = 0; + + //risingEdge.mode(Pullup); + risingEdge_L.rise(&pulses_L); + risingEdge_L.fall(&pulses_L); + + risingEdge_R.rise(&pulses_R); + risingEdge_R.fall(&pulses_R); + + while(1) { + + myled2 = 1; + count++; + uLCD.locate(0,1); + uLCD.printf("it's Ready, %d",count); + + if(uart.readable()) + { + uLCD.locate(5,2); + i = uart.getc(); + uLCD.printf("%c\n", i); + +/////////////////////////////////////// Go forward //////////////////////////// + if(i == 'f') + { + //speedA = speed_h; + + uLCD.locate(0,3); + uLCD.printf("Go Forward!"); + + m_L.speed(sA); + m_R.speed(sB); + + count_L = 0; + count_R = 0; + + while(!(uart.readable())) + { + temp_L = count_L; + temp_R = count_R; + + uLCD.locate(0,4); + uLCD.printf("Count_L: %d", temp_L); + + uLCD.locate(0,5); + uLCD.printf("Count_R: %d", temp_R); + + dist_L = (circumference/8)*((double)temp_L); + dist_R = (circumference/8)*((double)temp_R); + + uLCD.locate(0,7); + uLCD.printf("distance_L: %0.2f cm", dist_L); + uLCD.locate(0,8); + uLCD.printf("distance_R: %0.2f cm", dist_R); + } + + + + } +/////////////////////////////////////// Go Backward //////////////////////////// + if(i == 'b') + { + //speedA = speed_h; + + // uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Go Backward!"); + m_L.speed(-sA); + m_R.speed(-sB); + + } +/////////////////////////////////////// Turn left //////////////////////////// + if(i == 'l') + { + // speedA = speed_m; + + uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Turn Left!"); + m_L.speed(-0.65); + m_R.speed(0.67); + + + count_L = 0; + count_R = 0; + + while(!(uart.readable())) + { + if((count_L < 4) || (count_R <4)) + { + /* + temp_L = count_L; + temp_R = count_R; + + uLCD.locate(0,4); + uLCD.printf("Count_L: %d", temp_L); + + uLCD.locate(0,5); + uLCD.printf("Count_R: %d", temp_R); + + dist_L = (circumference/8)*((double)temp_L); + dist_R = (circumference/8)*((double)temp_R); + + uLCD.locate(0,7); + uLCD.printf("distance_L: %0.2f cm", dist_L); + uLCD.locate(0,8); + uLCD.printf("distance_R: %0.2f cm", dist_R); + */ + } + else + { + m_L.speed(0); + m_R.speed(0); + wait(0.5); + //m_L.speed(sA); + //m_R.speed(sB); + //wait(4); + } + } + } + +/////////////////////////////////////// Turn Right //////////////////////////// + + if(i == 'r') + { + //speedA = speed_m; + + uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Turn Right!"); + m_L.speed(0.65); + m_R.speed(-0.67); + + count_L = 0; + count_R = 0; + + while(!(uart.readable())) + { + if((count_L < 5)||(count_R < 5)) + { + /* + temp_L = count_L; + temp_R = count_R; + + uLCD.locate(0,4); + uLCD.printf("Count_L: %d", temp_L); + + uLCD.locate(0,5); + uLCD.printf("Count_R: %d", temp_R); + + dist_L = (circumference/8)*((double)temp_L); + dist_R = (circumference/8)*((double)temp_R); + + uLCD.locate(0,7); + uLCD.printf("distance_L: %0.2f cm", dist_L); + uLCD.locate(0,8); + uLCD.printf("distance_R: %0.2f cm", dist_R); + */ + } + else + { + m_L.speed(0); + m_R.speed(0); + wait(0.5); + //m_L.speed(sA); + //m_R.speed(sB); + //wait(4); + } + } + } + + + ////////////////////////////Tilt to the left ////////////////////////////// + + if(i == 'a') + { + //speedB = speedB - 0.1; + + //uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Tilt to the left!"); + + myled1 = 0; + myled2 = 1; + myled3 = 0; + myled4 = 0; + + m_L.speed(sA-0.04); + m_R.speed(sB+0.02); + + count_L = 0; + count_R = 0; + + + } + + ///////////////////////////Tilt to the right/////////////////////////// + + if(i == 'z') + { + + //speedB = speedB - 0.1; + + //uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Tilt to the right!"); + m_L.speed(sA+0.00); + m_R.speed(sB-0.05); + + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 1; + + count_L = 0; + count_R = 0; + + } + + + ////////////////Tilt angle to the left(CCW)/////////////// + + if(i == 'g') + { + + //uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Turn CCW a bit more!"); + + m_L.speed(0); + m_R.speed(0.8); + wait(0.1); + m_L.speed(0); + m_R.speed(0); + + + } + + + //////////////////Tilt angle to the right (CW)//////////////////////// + + if(i == 'h') + { + //uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Turn CW a bit more!"); + + m_L.speed(0.8); + m_R.speed(0); + wait(0.1); + m_L.speed(0); + m_R.speed(0); + + + + } + + ///////////////////////////Parked /////////////////////////// + if(i == 'p') + { + uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("stoped"); + wait(0.5); + m_L.speed(0); + m_R.speed(0); + + count_L = 0; + count_R = 0; + + } + +/////////////////////////////////////// Parking #1, On the left //////////////////////////// + if(i == 'w') + { + uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Parking on left! Front Park!"); + + m_L.speed(sA); + m_R.speed(sB); + wait(0); + + m_L.speed(0); + m_R.speed(0); + wait(1); + count_L = 0; + count_R = 0; + + m_L.speed(-0.6); + m_R.speed(0.65); + + // Step 1: Turn left 90 degrees + while((count_L < 5)||(count_R < 5)) + { + + } + m_L.speed(0); + m_R.speed(0); + wait(.5); + count_L = 0; + count_R = 0; + + + m_L.speed(0); + m_R.speed(0); + count_L = 0; + count_R = 0; + + + } +/////////////////////////////////////// Parking #2, On the right //////////////////////////// + if(i == 'x') + { + uLCD.cls(); + uLCD.locate(0,3); + uLCD.printf("Parking on Right! Front Park!"); + + m_L.speed(sA); + m_R.speed(sB); + wait(0); + + m_L.speed(0); + m_R.speed(0); + wait(1); + count_L = 0; + count_R = 0; + + m_L.speed(0.6); + m_R.speed(-0.65); + + // Step 1: Turn left 90 degrees + while((count_L < 5)||(count_R < 5)) + { + + } + m_L.speed(0); + m_R.speed(0); + wait(.5); + count_L = 0; + count_R = 0; + + + m_L.speed(0); + m_R.speed(0); + count_L = 0; + count_R = 0; + + + } + + + } + + else + { + myled1 = 0; + } + + + /* + + t.reset(); + t.start(); + count_L = 0; + count_R = 0; + + while(t.read_ms() < 251) { ; } + + t.stop(); + + long int temp_L = count_L; + long int temp_R = count_R; + + uLCD.locate(0,1); + uLCD.printf("Count_L: %d", temp_L); + + uLCD.locate(0,8); + uLCD.printf("Count_R: %d", temp_R); + + double circumference = 0.065 * 3.1416; // 6 cm wheel diameter * pi + + double rev_L = (double)(temp_L/4); + double rpm_L = ((double)temp_L)*15; + double speed_L = (circumference * ((double)temp_L))/4; + + double rev_R = (double)(temp_R/4); + double rpm_R = ((double)temp_R)*15; + double speed_R = (circumference * ((double)temp_R))/4; + + + uLCD.locate(0,3); + uLCD.printf("L: %0.2f RPM", rpm_L); + + uLCD.locate(0,4); + uLCD.printf("L: speed: %f m/s", speed_L); + + uLCD.locate(0,2); + uLCD.printf("Ct_nr_L: %d", (count_nr_L/4)); + + uLCD.locate(0,10); + uLCD.printf("R: %0.2f RPM", rpm_R); + + uLCD.locate(0,11); + uLCD.printf("R: speed: %f m/s", speed_R); + + uLCD.locate(0,9); + uLCD.printf("Ct_nr_R: %d", (count_nr_R/4)); + + */ + + //pc.putc(0xA); + //pc.putc(0xD); + } +} \ No newline at end of file