Torsten Wylegala
/
parallel_parking
foo
Fork of parallel_parking by
Diff: main.cpp
- Revision:
- 1:719ac5a42c19
- Parent:
- 0:c871d5355b99
- Child:
- 2:fae63e87900e
diff -r c871d5355b99 -r 719ac5a42c19 main.cpp --- a/main.cpp Sun Feb 07 06:14:32 2016 +0000 +++ b/main.cpp Mon Feb 08 11:13:44 2016 +0000 @@ -1,40 +1,139 @@ #include <mbed.h> +#include "rtos.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; +Timer timer; void hearbeat_thread(void const *args); int main() { + serialMinnow.baud(115200); + I2C *i2c = new I2C(p9, p10); + drivePWM.period_ms(20); steerPWM.period_ms(20); SystemTimer *millis = new SystemTimer(); - SupportSystem *supportSystem = new SupportSystem(0x80, &i2c); + SupportSystem *supportSystem = new SupportSystem(0x80, i2c); + Thread hearbeatThread(hearbeat_thread); + + steerPWM.pulsewidth_us(1500); + float strecke_anfang, strecke_ende, size_parking_space, zeit_anfang, zeit_ende, zeit_parking_space; + timer.start(); - ledTwo =1; - ledThree = 1; - steerPWM.pulsewidth_us(1500); - //float strecke_anfang, strecke_ende, size_parking_space; + // Konfiguration AMF-IMU + // [0]: Conversation Factor + // [1]: Sensor Position X + // [2]: Sensor Position Y + // [3]: Sensor Position Angle + //float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f}; + //supportSystem->writeImuConfig(configData); + +/* while(true){ + + ledTwo = !ledTwo; + IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); + + + serialMinnow.printf("X %f\n\r", IMU_registerDataBuffer->distanceXRegister); + serialMinnow.printf("S %f\n\r", IMU_registerDataBuffer->squal); + serialMinnow.printf("Uptime %d\n\r", supportSystem->readMaintenanceUptimeMillis()); + serialMinnow.printf("Error: %d\n\r", supportSystem->getTransmissionErrorCount()); + + wait(0.100); + } +*/ while(true){ - //IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); + ledTwo = !ledTwo; + IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); + if(buttonFour == 1){ - ledTwo = 0; + + drivePWM.pulsewidth_us(1700); + while(true){ + if(redlight == 1){ + ledThree = 1; + + /*do{ + + }while(redlight == 1); + strecke_anfang = IMU_registerDataBuffer->distanceXRegister; + ledThree = 1; + do{ + + }while(redlight == 0);*/ + + /*strecke_anfang = IMU_registerDataBuffer->distanceXRegister; + wait_ms(2000); + strecke_ende = IMU_registerDataBuffer->distanceXRegister; + size_parking_space = strecke_ende - strecke_anfang; + serialMinnow.printf("%f, ", IMU_registerDataBuffer->distanceXRegister); + serialMinnow.printf("%f\r\n", size_parking_space);*/ + + do{ + + }while(redlight == 1); + zeit_anfang = timer.read_ms(); + do{ + + }while(redlight == 0); + zeit_ende = timer.read_ms(); + zeit_parking_space = zeit_ende - zeit_anfang; + serialMinnow.printf("Differenz: %f\r\n", zeit_parking_space); + } + if(zeit_parking_space < 1350 && zeit_parking_space > 1100){ + + drivePWM.pulsewidth_us(1700); //Stück nach vorne fahren + wait_ms(200); + 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); + break; + } + } + } + if(buttonOne == 1){ + ledTwo = 0; + ledThree = 0; + drivePWM.pulsewidth_us(1500); + } + wait_ms(100); + } +} + + +void hearbeat_thread(void const *args) { + while(true) { + heartbeatLED = !heartbeatLED; + Thread::wait(100); + } +} + + + //------- 1 ---------------- /* drivePWM.pulsewidth_us(1700); //Vorwärts wait_ms(2500); @@ -50,7 +149,7 @@ wait_ms(420); drivePWM.pulsewidth_us(1500); //Ende steerPWM.pulsewidth_us(1500); - ledTwo = 1;*/ + ledTwo = 1; //------- 2 ---------------- drivePWM.pulsewidth_us(1800); //Vorwärts wait_ms(825); @@ -66,42 +165,4 @@ 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); - } -} + */ \ No newline at end of file