This is the source code used by master vehicle. Modules: CAMCTL SRVCTL MTRCTL still missing i2c readings from accelerometer and rf communication
Dependencies: CAMCTL MTRCTL SRVCTL mbed FXOS8700CQ XBEE
main.cpp@0:2a825db40e1b, 2014-10-14 (annotated)
- Committer:
- JalilChavez
- Date:
- Tue Oct 14 04:00:40 2014 +0000
- Revision:
- 0:2a825db40e1b
- Child:
- 1:a457798861a4
Master vehicle system
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JalilChavez | 0:2a825db40e1b | 1 | #include "camctl.h" |
JalilChavez | 0:2a825db40e1b | 2 | #include "srvctl.h" |
JalilChavez | 0:2a825db40e1b | 3 | #include "mtrctl.h" |
JalilChavez | 0:2a825db40e1b | 4 | |
JalilChavez | 0:2a825db40e1b | 5 | DigitalOut MAIN__xPinAlive( LED_BLUE ); |
JalilChavez | 0:2a825db40e1b | 6 | Serial xComPort1(USBTX,USBRX); |
JalilChavez | 0:2a825db40e1b | 7 | |
JalilChavez | 0:2a825db40e1b | 8 | |
JalilChavez | 0:2a825db40e1b | 9 | /********************************************************************************** |
JalilChavez | 0:2a825db40e1b | 10 | * CAMCTL-Module |
JalilChavez | 0:2a825db40e1b | 11 | ***********************************************************************************/ |
JalilChavez | 0:2a825db40e1b | 12 | uint16_t au16ThresholdBuffer[BUFFER_LENGTH] = {0}; |
JalilChavez | 0:2a825db40e1b | 13 | uint16_t *pu16DataBuffer; |
JalilChavez | 0:2a825db40e1b | 14 | uint8_t u8Index = 0; |
JalilChavez | 0:2a825db40e1b | 15 | uint8_t u8FrameCtr = 0; |
JalilChavez | 0:2a825db40e1b | 16 | /* Variables to get the line center */ |
JalilChavez | 0:2a825db40e1b | 17 | uint32_t u32Sum = 0; |
JalilChavez | 0:2a825db40e1b | 18 | uint32_t u32Avg = 0; |
JalilChavez | 0:2a825db40e1b | 19 | uint32_t u3SampleCtr = 0; |
JalilChavez | 0:2a825db40e1b | 20 | bool boCalibrationDone = false; |
JalilChavez | 0:2a825db40e1b | 21 | |
JalilChavez | 0:2a825db40e1b | 22 | int main() |
JalilChavez | 0:2a825db40e1b | 23 | { |
JalilChavez | 0:2a825db40e1b | 24 | /* Initialization of modules */ |
JalilChavez | 0:2a825db40e1b | 25 | CAMCTL_vInit(); |
JalilChavez | 0:2a825db40e1b | 26 | MTRCTL_vInit(); |
JalilChavez | 0:2a825db40e1b | 27 | SRVCTL_vInit(); |
JalilChavez | 0:2a825db40e1b | 28 | xComPort1.baud(115200); |
JalilChavez | 0:2a825db40e1b | 29 | /* Main loop */ |
JalilChavez | 0:2a825db40e1b | 30 | while(true) |
JalilChavez | 0:2a825db40e1b | 31 | { |
JalilChavez | 0:2a825db40e1b | 32 | /* Wait until camera is calibrated */ |
JalilChavez | 0:2a825db40e1b | 33 | if(boCalibrationDone == false) |
JalilChavez | 0:2a825db40e1b | 34 | { |
JalilChavez | 0:2a825db40e1b | 35 | boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 ); |
JalilChavez | 0:2a825db40e1b | 36 | } |
JalilChavez | 0:2a825db40e1b | 37 | else |
JalilChavez | 0:2a825db40e1b | 38 | { |
JalilChavez | 0:2a825db40e1b | 39 | /* Get frame */ |
JalilChavez | 0:2a825db40e1b | 40 | CAMCTL_vTriggerOneShotCapture(); |
JalilChavez | 0:2a825db40e1b | 41 | pu16DataBuffer = CAMCTL_pu16GetData(); |
JalilChavez | 0:2a825db40e1b | 42 | while( pu16DataBuffer == NULL ) |
JalilChavez | 0:2a825db40e1b | 43 | { |
JalilChavez | 0:2a825db40e1b | 44 | pu16DataBuffer = CAMCTL_pu16GetData(); |
JalilChavez | 0:2a825db40e1b | 45 | } |
JalilChavez | 0:2a825db40e1b | 46 | /* Detect path center */ |
JalilChavez | 0:2a825db40e1b | 47 | for( u8Index = 0; u8Index < BUFFER_LENGTH; u8Index++ ) |
JalilChavez | 0:2a825db40e1b | 48 | { |
JalilChavez | 0:2a825db40e1b | 49 | if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] ) |
JalilChavez | 0:2a825db40e1b | 50 | { |
JalilChavez | 0:2a825db40e1b | 51 | u32Sum = u32Sum + u8Index; |
JalilChavez | 0:2a825db40e1b | 52 | u3SampleCtr++; |
JalilChavez | 0:2a825db40e1b | 53 | |
JalilChavez | 0:2a825db40e1b | 54 | } |
JalilChavez | 0:2a825db40e1b | 55 | else |
JalilChavez | 0:2a825db40e1b | 56 | { |
JalilChavez | 0:2a825db40e1b | 57 | |
JalilChavez | 0:2a825db40e1b | 58 | } |
JalilChavez | 0:2a825db40e1b | 59 | } |
JalilChavez | 0:2a825db40e1b | 60 | /*Move servo according to camer readings*/ |
JalilChavez | 0:2a825db40e1b | 61 | u32Avg = (uint32_t)( u32Sum / u3SampleCtr ); |
JalilChavez | 0:2a825db40e1b | 62 | SRVCTL_vSetPosition( u32Avg, BUFFER_LENGTH ); |
JalilChavez | 0:2a825db40e1b | 63 | MTRCTL_vSetSystemSpeed( 400, (uint8_t)(u32Avg) ); |
JalilChavez | 0:2a825db40e1b | 64 | /* Report data through COM port */ |
JalilChavez | 0:2a825db40e1b | 65 | xComPort1.printf("u8CenterPos = %f\r\n",(float)(u32Avg/128.0)); |
JalilChavez | 0:2a825db40e1b | 66 | /* Reinitialize variables */ |
JalilChavez | 0:2a825db40e1b | 67 | u32Sum = 0; |
JalilChavez | 0:2a825db40e1b | 68 | u3SampleCtr = 0; |
JalilChavez | 0:2a825db40e1b | 69 | } |
JalilChavez | 0:2a825db40e1b | 70 | /* Pin alive message */ |
JalilChavez | 0:2a825db40e1b | 71 | MAIN__xPinAlive = !MAIN__xPinAlive; |
JalilChavez | 0:2a825db40e1b | 72 | wait(0.0001); |
JalilChavez | 0:2a825db40e1b | 73 | } |
JalilChavez | 0:2a825db40e1b | 74 | } |
JalilChavez | 0:2a825db40e1b | 75 |