Sungwoo Han
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