Torsten Wylegala
/
parallel_parking
foo
Fork of parallel_parking by
Diff: main.cpp
- Revision:
- 0:c871d5355b99
- Child:
- 1:719ac5a42c19
diff -r 000000000000 -r c871d5355b99 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Feb 07 06:14:32 2016 +0000 @@ -0,0 +1,107 @@ +#include <mbed.h> +#include "Periphery/SupportSystem.h" +#include "Misc/SystemTimer.h" +#include "rtos.h" + +#define PI 3.14159265 + +Serial serialXbee(p28,p27), serialMinnow(p13, p14); +PwmOut drivePWM(p22), steerPWM(p21); +I2C i2c(p9, p10); + +DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4); + +DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30); +DigitalIn redlight(p16); + +IMU_RegisterDataBuffer_t *IMU_registerDataBuffer; + +void hearbeat_thread(void const *args); + +int main() { + drivePWM.period_ms(20); + steerPWM.period_ms(20); + + SystemTimer *millis = new SystemTimer(); + SupportSystem *supportSystem = new SupportSystem(0x80, &i2c); + Thread hearbeatThread(hearbeat_thread); + + ledTwo =1; + ledThree = 1; + steerPWM.pulsewidth_us(1500); + //float strecke_anfang, strecke_ende, size_parking_space; + + while(true){ + //IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); + if(buttonFour == 1){ + ledTwo = 0; +//------- 1 ---------------- + /* drivePWM.pulsewidth_us(1700); //Vorwärts + wait_ms(2500); + drivePWM.pulsewidth_us(1500); //Stehen + wait_ms(200); + drivePWM.pulsewidth_us(1325); //1. Einschlag + steerPWM.pulsewidth_us(2000); + wait_ms(970); + steerPWM.pulsewidth_us(1000); //2. Einschlag + wait_ms(950); + drivePWM.pulsewidth_us(1700); //Korrektur nach vorne + steerPWM.pulsewidth_us(1700); + wait_ms(420); + drivePWM.pulsewidth_us(1500); //Ende + steerPWM.pulsewidth_us(1500); + ledTwo = 1;*/ +//------- 2 ---------------- + drivePWM.pulsewidth_us(1800); //Vorwärts + wait_ms(825); + drivePWM.pulsewidth_us(1500); //Stehen + wait_ms(200); + drivePWM.pulsewidth_us(1300); //1. Einschlag + steerPWM.pulsewidth_us(2000); + wait_ms(680); + steerPWM.pulsewidth_us(1000); //2. Einschlag + wait_ms(630); + drivePWM.pulsewidth_us(1700); //Korrektur nach vorne + steerPWM.pulsewidth_us(1700); + wait_ms(420); + drivePWM.pulsewidth_us(1500); //Ende + steerPWM.pulsewidth_us(1500); + + + } + if(buttonOne == 1){ + ledTwo = 0; + ledThree = 0; + drivePWM.pulsewidth_us(1500); + } + } + + + /*if(buttonFour == 1){ + drivePWM.pulsewidth_us(1700); + + if(redlight == 1){ + ledTwo = 1; + + do{ + ledThree = 1; + }while(redlight == 1); + strecke_anfang = IMU_registerDataBuffer->distanceXRegister; + do{ + ledThree = 0; + }while(redlight == 0); + strecke_ende = IMU_registerDataBuffer->distanceXRegister; + size_parking_space = strecke_ende - strecke_anfang; + + } + } + */ +} + + +void hearbeat_thread(void const *args) { + while(true) { + heartbeatLED = !heartbeatLED; + Thread::wait(100); + } +}