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 7:4cf73c284530, committed 2019-08-01
- Comitter:
- Kim_GiHoon
- Date:
- Thu Aug 01 05:05:02 2019 +0000
- Parent:
- 6:f8082c88b65d
- Commit message:
- HGU_SWprogram_by_GiHoon_for_others
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/A_oscar_ver_anglechange_for_msg_0620final.lib Thu Aug 01 05:05:02 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/YLK/code/roscar_ver_20190501_anglechange_copy/#f8082c88b65d
--- a/main.cpp Fri Jun 14 08:25:48 2019 +0000
+++ b/main.cpp Thu Aug 01 05:05:02 2019 +0000
@@ -10,11 +10,12 @@
#include <std_msgs/Float32.h>
#include "Servo.h"
#include "MPU6050.h"
+#include <geometry_msgs/Twist.h>
//#include "esc.h"
//#include "MbedJSONValue.h"
#define spd 60
-
+int flag_key=1;
//Serial uart(PA_11, PA_12, 57600);
ros::NodeHandle nh;
@@ -23,14 +24,16 @@
PwmOut sv(D5);
PwmOut esc1(D9);
+geometry_msgs::Twist cmd;
+
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 ANG=90;
+int DIR1='S';
int Num = 0;
char inData[80];
float dt,Yaw, lgyroZ=0;
@@ -57,124 +60,158 @@
mpu6050_yaw.data=Yaw;
}
-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')
+int spd_msg;
+int ang_msg;
+
+void dc_motor(int DIR1, float SPEED)
+{
+ float getPWM = SPEED/5000;
+ if(DIR1=='F')
{
for(int i=0; i<=SPEED; i++)
{
esc1 = (default_PWM + getPWM*i/SPEED);
- //wait_ms(20);
+ //wait_ms(20);
}
}
- else if(DIR1=='S')
+ else if(DIR1=='S')
{
esc1 = default_PWM;
- // wait_ms(20);
+ // wait_ms(20);
}
- else if(DIR1=='B')
+ else if(DIR1=='B')
{
for(int i=0; i<=SPEED; i++)
{
esc1 = (default_PWM - getPWM*i/SPEED);
//wait_ms(20);
}
+ }
+}
+
+void handlerFunction_key(const geometry_msgs::Twist& commandSend){
+ flag_key=0;
+ cmd= commandSend;
+ spd_msg=cmd.linear.x;
+ ang_msg=cmd.angular.z;
+ flag_key=cmd.linear.y;
+
+ if(ang_msg>=150)ang_msg=150;
+ if(ang_msg<=30)ang_msg=30;
+ if(spd_msg>=200)spd_msg=200;
+ if(spd_msg<=-200)spd_msg=-200;
+
+ if(spd_msg==0){
+ SPEED = spd_msg;
+ DIR1 ='S';
+ ANG = ang_msg;
}
+ else if(spd_msg>0){
+ SPEED = spd_msg;
+ DIR1 ='F';
+ ANG = ang_msg;
+ }
+ else{
+ SPEED = -spd_msg;
+ DIR1 ='B';
+ ANG = ang_msg;
+ }
+
+ // float getPWM = 0.063 + ((SPEED+200)/400.0) * 0.065 ;
+ //float getPWM = SPEED/5000.0;
+ dc_motor(DIR1, SPEED);
+
+ if (ANG!=0)
+ { //assign ANG value to the servo motor angle
+ float Angle_Value = 0;
+ Angle_Value = 0.06 + (ANG-30)/120.0 * (0.06);
+ sv.write(Angle_Value);
+ /*
+ 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);
+ }
+
}
- 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);
+void handlerFunction_Cam(const geometry_msgs::Twist& commandSend){
+ if(flag_key==true){
+ cmd= commandSend;
+ spd_msg=cmd.linear.x;
+ ang_msg=cmd.angular.z;
+ if(ang_msg>=150)ang_msg=150;
+ if(ang_msg<=30)ang_msg=30;
+ if(spd_msg>=200)spd_msg=200;
+ if(spd_msg<=-200)spd_msg=-200;
+
+ if(spd_msg==0){
+ SPEED = spd_msg;
+ DIR1 ='S';
+ ANG = ang_msg;
+ }
+ else if(spd_msg>0){
+ SPEED = spd_msg;
+ DIR1 ='F';
+ ANG = ang_msg;
}
- }
- 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);
- }
+ else{
+ SPEED = -spd_msg;
+ DIR1 ='B';
+ ANG = ang_msg;
+ }
+
+ //float getPWM = SPEED/5000.0;
+ dc_motor(DIR1, SPEED);
+
+ if (ANG != 0)
+ { //assign ANG value to the servo motor angle
+ float Angle_Value;
+
+ Angle_Value = 0.06 + 0.06 * (ANG - 30.0) / 120.0;
+ sv.write(Angle_Value);
+
+ past_ANG = ANG;
+ //wait(0.02);
+ }
+ }
}
-ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);
-
+//ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);
+ros::Subscriber<geometry_msgs::Twist> sub("/data_msg", &handlerFunction_Cam);
+ros::Subscriber<geometry_msgs::Twist> sub_key("/data_msg_key", &handlerFunction_key);
int main() {
nh.initNode();
nh.subscribe(sub);
+ nh.subscribe(sub_key);
nh.advertise(chatter);
nh.advertise(yaw_value);
esc1.period(0.01f);
esc1.write(default_PWM);
- sv.period(0.01f);
- sv.write(0.15);
+ sv.period(0.0164f);
+ //sv.write(0.09);
+ sv.write(0.09);
i2c.frequency(400000);
t.start();
--- a/rosserial_mbed_String_PubSub.lib Fri Jun 14 08:25:48 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/FernandoLG/code/rosserial_mbed_String_PubSub/#26006d815db0