void initSensors()
{
initBarometer();
initSonar();
initVoltage();
initGPS();
}
void init()
{
OSCCON = 0b01100010;
UART_Init();
initBargraph();
initSonar();
initBuzzer();
song();
CLRWDT();
//INIT OLED
I2C_Close(); // Close the I2C Bus
I2C_Init(1); // I2C 400kHz, 20MHz-CRYSTAL
Oled_Init();
Oled_SetFont(Terminal12x16, 12, 16, 32,127);
Oled_FillScreen(0x00, 0, 0);
Oled_ConstText("SONAR", 35, 0);
Oled_Text("!", 110, 0);
Oled_ConstText("L=", 2, 5);
Oled_ConstText("cm", 105, 5);
}
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Configure the system clock */
SystemClock_Config();
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_I2C3_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM5_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
MX_TIM11_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_Base_Start_IT(&htim11); // 100 msec timer
initSonar( MAX_SONAR);
initSerOutput();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
// >>>>> Sonar reading
triggerSonar( COUPLE_0_2);
HAL_Delay(49);
triggerSonar( COUPLE_1_3);
HAL_Delay(49);
// <<<<< Sonar reading
// >>>>> Serial Output
convertMeasures();
sendMeasures();
// <<<<< Serial Output
}
/* USER CODE END 3 */
}
/******************************************************
*
* Main; the master of functions, the definer of variables.
*
* ***************************************************/
int main( int argc, char **argv )
{
int i, j;
ros::init(argc, argv, "sonar");
ros::NodeHandle n;
//ros::Publisher nodenameVariablenameMsg = handle.outsidness<libraryname::type>("nodenameVariablename", bufflen?);
ros::Publisher sonarBearingMsg = n.advertise<std_msgs::Float32>("sonarBearing", 100);
ros::Publisher sonarBinsMsg = n.advertise<std_msgs::Float32>("sonarBins", 100);
std_msgs::Float32 sonarBearing;
std_msgs::Float32 sonarBins;
/* Open and Configure the Serial Port. */
open_port();
config_port();
//printf("dihqwdi %d\n", getU16(0x0A, 0x80));
/* Initilise the sonar */
initSonar();
/* optional, sendBBUser command, doesnt work :/ */
//sendBB();
/* Make and send the head parameters, currently defined at the top of this file */
headSetup();
/* ask for some data, will get datas */
for( i = 0; i < 200; i ++)
{
requestData();
//pass datas
sonarBearing.data = (float) bearing;
sonarBins.data = (float) bins;
//publish
sonarBearingMsg.publish(sonarBearing);
sonarBinsMsg.publish(sonarBins);
ros::spinOnce();
}
/* close file and exit program */
close(fd);
return 0;
}
void initAltitudeHold(void) {
#if USE_SONAR
initSonar();
#endif
#if USE_LIDAR_LITE
initLidarLite();
#endif
#if USE_BARO
initBMP180(&bmp180);
#endif
#if UART_DEBUG && USE_BARO
UARTprintf("Barometer values: %d\t%d\t%d\n", bmp180.pressure, bmp180.temperature, (int32_t)bmp180.groundAltitude);
#endif
}
/******************************************************
*
* Main; the master of functions, the definer of variables.
*
* ***************************************************/
int main( int argc, char **argv )
{
int i, j;
ros::init(argc, argv, "sonar");
ros::NodeHandle n;
//ros::Publisher nodenameVariablenameMsg = handle.outsidness<libraryname::type>("nodenameVariablename", bufflen?);
ros::Publisher sonarBearingMsg = n.advertise<std_msgs::Float32>("sonarBearing", 100);
ros::Publisher sonarBinsMsg = n.advertise<std_msgs::Float32>("sonarBins", 100);
ros::Publisher sonarBinsArrMsg = n.advertise<std_msgs::Int32MultiArray>("sonarBinsArr", 100);
ros::Publisher sonarScanMsg = n.advertise<sensor_msgs::LaserScan>("sonarScan", 100);
ros::Subscriber sub1 = n.subscribe("sonarCmd", 100, cmdCallback);
ros::Subscriber sub2 = n.subscribe("sonarRange", 100, rangeCallback);
ros::Subscriber sub3 = n.subscribe("sonarLeft", 100, leftCallback);
std_msgs::Float32 sonarBearing;
std_msgs::Float32 sonarBins;
std_msgs::Int32MultiArray sonarBinsArr;
sensor_msgs::LaserScan sonarScan;
//ros::Time scan_time = ros::Time::now();
//Set up a LaserScan message:
initLaserData(sonarScan);
/* Open and Configure the Serial Port. */
open_port();
if( tcgetattr(fd, &orig_terimos) < 0)
{
ROS_ERROR("Probably didn't get the serial port (is it connected?).\n");
return 0;
}
config_port();
//makePacket(mtReBoot);
//printf("dihqwdi %d\n", getU16(0x0A, 0x80));
/* Initilise the sonar */
initSonar();
/* optional, sendBBUser command, doesnt work :/ */
//sendBB();
while(ros::ok())
{
ros::Time scan_time = ros::Time::now();
switchCmd = 1;
//Stare LL
if(switchCmd == 0)
{
/* Make and send the head parameters, currently defined at the top of this file */
if(switchFlag == 0)
{
//SCANSTARE = 0x2B;
//RANGE = 5;
LEFTANGLE = 3300;
RIGHTANGLE = 3600;
headSetup();
switchFlag = 1;
ROS_INFO("Stare Initilized");
}
// i = 0;
//makePacket(mtStopAlive);
/* ask for some data, will get datas */
// while(i < 30)
// {
requestData();
//
//tcflush(fd, TCIFLUSH);//remove
//pass datas
sonarBearing.data = (float) bearing;
//printf("%d", bearing);
sonarBins.data = (float) bins;
sonarBinsArr.data.clear();
for (int k = 0; k < 90; k++)
{
sonarBinsArr.data.push_back(tempBinArray[k]);
}
//publish
sonarBearingMsg.publish(sonarBearing);
sonarBinsMsg.publish(sonarBins);
sonarBinsArrMsg.publish(sonarBinsArr);
//ROS_INFO("Bearing: %f, Bins: %f", bearing, bins);
//printf("%d - %d\n", bearing, bins);
//ros::spinOnce();
i++;
// }
}
//scanner
else
{
/* Make and send the head parameters, currently defined at the top of this file */
if(switchFlag == 0)
{
SCANSTARE = 0xC0;
RANGE = 45;
LEFTANGLE = 0;
RIGHTANGLE = 6399;
headSetup();
switchFlag = 1;
ROS_INFO("Scanner Activated");
}
i = 0;
//makePacket(mtStopAlive);
/* ask for some data, will get datas */
// while(i < 30)
// {
requestData();
//
//tcflush(fd, TCIFLUSH);//remove
//pass datas
/* Hack to fix the bearing mess up between ~3327-3599, dafuq. */
//printf("%d - ", bearing);
if(sonarDirection == UP)
{
if( bearingOld != bearing - (STEPANGLE * 2) && bearingOld != 0)
{
if((bearing >= 0 && bearing <= 100) && (bearingOld <= 6399 && bearingOld >= 6300))
bearingFail = 0;
else
bearingFail = 1;
}
if( (bearing > bearingOld + 700) && bearingFail == 1)
bearingFail = 0;
}
else
{
if( bearingOld != bearing + (STEPANGLE * 2) && bearingOld != 0)
{
if((bearingOld >= 0 && bearingOld <= 100) && (bearing <= 6399 && bearing >= 6300))
bearingFail = 0;
else
bearingFail = 1;
}
if( (bearing > bearingOld + 700) && bearingFail == 1)
bearingFail = 0;
}
if(bearingFail == 1)
{
//bearing = bearing + 768;
sonarBearing.data = (float) bearing + 768;
}
else
{
sonarBearing.data = (float) bearing;
}
//printf("%d -- %d\n", bearing, bearingFail);
/* end fix */
//sonarBearing.data = (float) bearing;
sonarBins.data = (float) bins;
sonarBinsArr.data.clear();
for (int k = 0; k < 90; k++)
{
sonarBinsArr.data.push_back(tempBinArray[k]);
}
// Create laser scan data from the sonar bins array.
createLaserData(sonarScan, tempBinArray, scan_time);
//publish
sonarBearingMsg.publish(sonarBearing);
sonarBinsMsg.publish(sonarBins);
sonarBinsArrMsg.publish(sonarBinsArr);
sonarScanMsg.publish(sonarScan);
//ROS_INFO("Bearing: %f, Bins: %f", bearing, bins);
//printf("%d - %d\n", bearing, bins);
//ros::spinOnce();
i++;
bearingOld = bearing;
// }
}
ros::spinOnce();
//sleep(1);
}
/* close file and exit program */
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &(orig_terimos));
close(fd);
return 0;
}
