fyp base control
Dependencies: mbed mbed-rtos ros_lib_melodic_
Diff: main.cpp
- Revision:
- 1:782534ae7166
- Parent:
- 0:54452c8078db
- Child:
- 3:5402a080282c
diff -r 54452c8078db -r 782534ae7166 main.cpp --- a/main.cpp Fri Jul 29 14:42:35 2022 +0000 +++ b/main.cpp Sat Jul 30 18:05:58 2022 +0000 @@ -1,7 +1,7 @@ #define BAUD 115200 #include "mbed.h" #include "math.h" -//#include "rtos.h" +#include "rtos.h" #include <ros.h> #include "std_msgs/Bool.h" #include "std_msgs/String.h" @@ -14,29 +14,58 @@ NodeHandle nh; +Thread battery_thread; + +AnalogIn battery(A0); DigitalOut myled = LED1; // -> Motor PWN Pin -PwmOut motor_pwm[] = {D6, D5, D4, D3}; +PwmOut motor_pwm[] = {D6, D5, D3, D4}; DigitalOut LED01 = D7; DigitalOut LED02 = D8; DigitalOut LED03 = D9; //motor number -void antiturning(float a, float b){ - motor_pwm[0]=0; - motor_pwm[1]=a; - motor_pwm[2]=0; - motor_pwm[3]=b; +void turn(float r, float l){ + float a, b, c, d; + if (l < 0){ + a = 0.0; + b = -l; + } + else{ + a = l; + b = 0.0; + } + if (r < 0){ + c = 0.0; + d = -r; + } + else{ + c = r; + d = 0.0; + } + motor_pwm[0]=a; + motor_pwm[1]=b; + motor_pwm[2]=c; + motor_pwm[3]=d; } -void turning(float a, float b){ - motor_pwm[0]=a; - motor_pwm[1]=0; - motor_pwm[2]=b; - motor_pwm[3]=0; +void resize(float &a, float &b){ + if (a != 0 or b != 0) { + if (b > a){ + a = a/b; + if (b > 1) + b = 1; + } + else{ + b = b/a; + if (a > 1) + a = 1; + } + } + } - + void r1Callback(const Bool& _msg){ bool check = _msg.data; if(check){ @@ -49,83 +78,94 @@ LED03 = 0;} } +float sum = 0.0; +//std_msgs::String motor_msg; +//char buffer[50]; +//Publisher motor_pub("motor", &motor_msg); + + void twCallback(const Twist& _msg) { float x = _msg.linear.x; float y = _msg.linear.y; + sum =abs(x)+abs(y); + resize(y, sum); + if (y < 0) + sum = -sum; + if (x < 0) + turn(y, sum); + else + turn(sum, y); + +// sprintf(buffer, "turn: %f, %f", sum, y); +// motor_msg.data = buffer; +// motor_pub.publish(&motor_msg); - if (x==0.0){ - if(y<0){ - antiturning(abs(y), abs(y)); - } - else{ - turning(y, y); - } - } - else{ - float angle = atan(abs(y)/abs(x)); - float angle45 = atan(1.0); - if(x<0){ - if(angle <= angle45){ - motor_pwm[0]=0; - motor_pwm[1]=abs(x + abs(y)); - motor_pwm[2]=-x; - motor_pwm[3]=0; - - } - else{ - if(y<0){ - antiturning(-y, -y+x);} - else{ - turning(y+x, y);} - } - } - else { - if(angle <= angle45){ - motor_pwm[0]=x; - motor_pwm[1]=0; - motor_pwm[2]=0; - motor_pwm[3]=x - abs(y); - - } - else{ - if(y<0){ - antiturning(-y-x,-y);} - else{ - turning(y, y-x);} - } - } - } -// if(y<0) -// { -// motor_pwm[2]=0; -// motor_pwm[3]=abs(y); +// +// if (x==0.0){ +// turn(y,y); // } -// else -// { -// motor_pwm[2]=y; -// motor_pwm[3]=0; +// else{ +// float angle = atan(abs(y)/abs(x)); +// float angle45 = atan(1.0); +// if(x<0){ +// if(angle <= angle45){ +// motor_pwm[0]=0; +// motor_pwm[1]=abs(x + abs(y)); +// motor_pwm[2]=-x; +// motor_pwm[3]=0; +// +// } +// else{ +// if(y<0){ +// antiturning(-y, -y+x);} +// else{ +// turning(y+x, y);} +// } +// } +// else { +// if(angle <= angle45){ +// motor_pwm[0]=x; +// motor_pwm[1]=0; +// motor_pwm[2]=0; +// motor_pwm[3]=x - abs(y); +// +// } +// else{ +// if(y<0){ +// antiturning(-y-x,-y);} +// else{ +// turning(y, y-x);} +// } +// } // } -// if(x<0) -// { -// motor_pwm[0]=0; -// motor_pwm[1]=abs(x); -// -// } -// else -// { -// motor_pwm[0]=x; -// motor_pwm[1]=0; -// } } +std_msgs::String str_msg; +Publisher pub("battery", &str_msg); Subscriber<Twist> subtw("base_twist", &twCallback); Subscriber<Bool> subr1("bt_r1", &r1Callback); + +void battery_pub(){ + char buf[10]; + while(1){ + + sprintf(buf, "%d", int(((battery.read()*3.3f) - (6/2.68))/(2.4/268))); +// sprintf(buf,"%f",battery.read()); + str_msg.data = buf; + pub.publish(&str_msg); +// nh.spinOnce(); + Thread::wait(1000); + + } +} + int main() { //Rate rate(10); nh.getHardware()->setBaud(BAUD); nh.initNode(); - + nh.advertise(pub); +// nh.advertise(motor_pub); nh.subscribe(subtw); nh.subscribe(subr1); @@ -133,7 +173,8 @@ motor_pwm[1]=0; motor_pwm[2]=0; motor_pwm[3]=0; - + + battery_thread.start(battery_pub); while (1) { nh.spinOnce();