GiHoon Kim
/
A_cseecar_ver_pub
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Servo roscar_ver_20190501_anglechange_copy ros_lib_kinetic roscar_ver_20190501_copy
Revision 5:daa981fdd231, committed 2019-06-14
- Comitter:
- YLK
- Date:
- Fri Jun 14 08:19:00 2019 +0000
- Parent:
- 4:26006d815db0
- Child:
- 6:f8082c88b65d
- Commit message:
- clone
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050IMU.lib Fri Jun 14 08:19:00 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/The-A-Team/code/MPU6050IMU/#3a3ef233f1b4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Jun 14 08:19:00 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
--- a/main.cpp Fri Aug 24 21:21:46 2018 +0000
+++ b/main.cpp Fri Jun 14 08:19:00 2019 +0000
@@ -2,35 +2,208 @@
* rosserial Subscriber Example
* Blinks an LED on callback
*/
+
#include "mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/Float32.h>
+#include "Servo.h"
+#include "MPU6050.h"
+//#include "esc.h"
+//#include "MbedJSONValue.h"
+#define spd 60
+
+
+//Serial uart(PA_11, PA_12, 57600);
ros::NodeHandle nh;
DigitalOut myled(LED1);
+//PWM
+PwmOut sv(D5);
+PwmOut esc1(D9);
-std_msgs::String commandRead;
+std_msgs::Float32 mpu6050_yaw;
+std_msgs::Int8 commandRead;
ros::Publisher chatter("chatter", &commandRead);
+ros::Publisher yaw_value("yaw_value", &mpu6050_yaw);
+
+int SPEED=0;
+int ANG=0;
+char DIR1='S';
+int Num = 0;
+char inData[80];
+float dt,Yaw, lgyroZ=0;
+int past_ANG = 90;
+int gyroZ_raw= 0;
+float default_PWM = 0.15;
+
+MPU6050 mpu6050;
+Timer t;
+
+void MPU6050_calculation()
+{
+ if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01)
+ { // check if data ready interrupt
+ mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
+ mpu6050.getGres();
+ gyroZ_raw = (int)gyroCount[2]*gRes; // - gyroBias[2];
+ }
+ Now = t.read_us();
+ dt = (float)((Now - lastUpdate)/1000000.0f) ;
+ lastUpdate = Now;
+ Yaw += ((lgyroZ+gyroZ_raw)/2)*dt;
+ lgyroZ = gyroZ_raw;
+ mpu6050_yaw.data=Yaw;
+}
-// /*
-void handlerFunction(const std_msgs::String& commandSend){
+void handlerFunction(const std_msgs::Int8& commandSend){
+
commandRead = commandSend;
+ switch (commandSend.data)
+ {
+ case 1: //forward
+ SPEED =spd;
+ DIR1 = 'F';
+ ANG = 90;
+ break;
+ case 2: //strong left
+ SPEED =spd;
+ DIR1 = 'F';
+ ANG = 45;
+ break;
+ case 3: //strong right
+ SPEED =spd;
+ DIR1 = 'F';
+ ANG = 135;
+ break;
+ case 4: // weak left
+ SPEED=spd;
+ ANG = 75;
+ DIR1 = 'F';
+ break;
+ case 5: //weak right
+ SPEED = spd;
+ DIR1 = 'F';
+ ANG =105;
+ break;
+ case 6: // mid left
+ SPEED = spd;
+ DIR1 = 'F';
+ ANG =60;
+ break;
+ case 7: //mid right
+ SPEED = spd;
+ DIR1 = 'F';
+ ANG =120;
+ break;
+ case 8: //stop
+ SPEED = 1;
+ DIR1 ='S';
+ ANG = 90;
+ break;
+ default :
+ SPEED = 1;
+ DIR1 ='S';
+ ANG = 90;
+ break;
+ }
+ if (SPEED)
+ { //assign speed value to Motor_PWM
+ float getPWM = SPEED/5000.0;
+ if(DIR1=='F')
+ {
+ for(int i=0; i<=SPEED; i++)
+ {
+ esc1 = (default_PWM + getPWM*i/SPEED);
+ //wait_ms(20);
+ }
+ }
+ else if(DIR1=='S')
+ {
+ esc1 = default_PWM;
+ // wait_ms(20);
+ }
+ else if(DIR1=='B')
+ {
+ for(int i=0; i<=SPEED; i++)
+ {
+ esc1 = (default_PWM - getPWM*i/SPEED);
+ //wait_ms(20);
+ }
+ }
+ }
+
+ if (ANG)
+ { //assign ANG value to the servo motor angle
+ float Angle_Value = 0;
+ if(ANG>=past_ANG)
+ {
+ for(int i=past_ANG; i<=ANG; i++)
+ {
+ Angle_Value = 0.05+i/90.0*0.1;
+ sv.write(Angle_Value);
+ //wait(0.02);
+ }
+ }
+ else if(past_ANG >= ANG)
+ {
+ for(int i=past_ANG; i>=ANG; i--)
+ {
+ Angle_Value = 0.05+i/90.0*0.1;
+ sv.write(Angle_Value);
+ //wait(0.02);
+ }
+ }
+ past_ANG = ANG;
+ //wait(0.02);
+ }
+
}
-// */
-ros::Subscriber<std_msgs::String> sub("cmd_vel", &handlerFunction);
+ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);
int main() {
+
nh.initNode();
nh.subscribe(sub);
+
nh.advertise(chatter);
+ nh.advertise(yaw_value);
+
+ esc1.period(0.01f);
+ esc1.write(default_PWM);
+ sv.period(0.01f);
+ sv.write(0.15);
+ i2c.frequency(400000);
+ t.start();
+
+ uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // read WHO_AM_I register
- char initialValue[2] = "X";
- commandRead.data= initialValue;
+ if (whoami == 0x68)
+ {
+ mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+ if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
+ {
+ mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+ mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ mpu6050.initMPU6050();
+ }
+ }
+ else {
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+ wait_ms(100);
+
while (1) {
- chatter.publish( &commandRead);
+ //Drive_Contrl();
+ MPU6050_calculation();
+
+ yaw_value.publish(&mpu6050_yaw);
+ chatter.publish(&commandRead);
+
nh.spinOnce();
- wait_ms(10);
+ wait_ms(100);
}
-}
+}
\ No newline at end of file
--- a/mbed.bld Fri Aug 24 21:21:46 2018 +0000 +++ b/mbed.bld Fri Jun 14 08:19:00 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539 \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rosserial_mbed_String_PubSub.lib Fri Jun 14 08:19:00 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/FernandoLG/code/rosserial_mbed_String_PubSub/#26006d815db0