Madpulse ROS Code
Dependencies: mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn
Diff: main.cpp
- Revision:
- 2:899128d20215
- Parent:
- 0:42d1dda7d9c0
- Child:
- 3:82e223a4a4e4
--- a/main.cpp Tue Jul 12 19:16:19 2016 +0000 +++ b/main.cpp Wed Aug 17 20:31:42 2016 +0000 @@ -19,7 +19,7 @@ DigitalOut status_LED(LED1); DigitalOut armed_LED(LED2); DigitalOut auto_LED(LED3); -DigitalOut hall_LED(LED4); +DigitalOut imu_LED(LED4); Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc Serial xbee(p28, p27); // tx, rx for Xbee @@ -67,48 +67,41 @@ pc.printf("BNO055 connected\r\n"); imu.setmode(OPERATION_MODE_CONFIG); imu.SetExternalCrystal(1); - //bno.set_orientation(1); imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer - //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer - imu.set_angle_units(DEGREES); - imu.set_mapping(1); + imu.set_angle_units(RADIANS); + imu.set_mapping(2); + + } else { pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); status_LED = 1; armed_LED = 1; - hall_LED = 1; + imu_LED = 1; auto_LED = 1; while(1) { status_LED = !status_LED; armed_LED = !armed_LED; - hall_LED = !hall_LED; + imu_LED = !imu_LED; auto_LED = !auto_LED; wait(0.5); } } - pc.printf("ES456 Vehcile Program\r\n"); + pc.printf("ES456 Vehicle Sensor Logger\r\n"); while(1){ - imu.get_angles(); imu.get_accel(); imu.get_gyro(); imu.get_mag(); if(t.read()-t_imu > (1/LOG_RATE)) { - - // pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); - pc.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw); xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); - xbee.printf("$BNO,%.2f, %.2f, %.2f\r\n",imu.euler.roll,imu.euler.pitch,imu.euler.yaw); - xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); - t_imu = t.read(); } // if t.read wait(1/LOOP_RATE);