![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fyp base control
Dependencies: mbed mbed-rtos ros_lib_melodic_
main.cpp
- Committer:
- howanglam3
- Date:
- 23 months ago
- Revision:
- 4:e6db600f9a9f
- Parent:
- 3:5402a080282c
File content as of revision 4:e6db600f9a9f:
#define BAUD 256000 #include "mbed.h" #include "math.h" #include "rtos.h" #include <ros.h> #include "std_msgs/Bool.h" #include "std_msgs/String.h" #include "geometry_msgs/Twist.h" using namespace ros; using namespace std_msgs; using namespace geometry_msgs; NodeHandle nh; Thread battery_thread; AnalogIn battery(A0); DigitalOut myled = LED1; // -> Motor PWN Pin PwmOut motor_pwm[] = {D6, D5, D3, D4}; DigitalOut LED01 = D7; DigitalOut LED02 = D8; DigitalOut LED03 = D9; //motor number double round(double number) { return number < 0.0 ? ceil(number - 0.5): floor(number + 0.5); }; void turn(float r, float l){ float a, b, c, d; if (l < 0){ a = 0.0; b = abs(l); } else{ a = l; b = 0.0; } if (r < 0){ c = 0.0; d = abs(r); } else{ c = r; d = 0.0; } motor_pwm[0]=a; motor_pwm[1]=b; motor_pwm[2]=c; motor_pwm[3]=d; } 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){ LED01 = 1; LED02 = 1; LED03 = 1;} else{ LED01 = 0; LED02 = 0; 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 = round(_msg.linear.x*100.0)/100.0; float y = round(_msg.linear.y*100.0)/100.0; sum =abs(x)+abs(y); resize(y, sum); if (x <= 0){ if (y<=0){ motor_pwm[0]= 0; motor_pwm[1]= abs(y); motor_pwm[2]= 0; motor_pwm[3]= sum; //turn(y, sum); } else{ motor_pwm[0]= sum; motor_pwm[1]= 0; motor_pwm[2]= y; motor_pwm[3]= 0; } } else{ if (y<=0){ motor_pwm[0]= 0; motor_pwm[1]= sum; motor_pwm[2]= 0; motor_pwm[3]= abs(y); } else{ motor_pwm[0]= y; motor_pwm[1]= 0; motor_pwm[2]= sum; motor_pwm[3]= 0; } } // turn(sum, y); // sprintf(buffer, "turn: %f, %f", sum, y); // motor_msg.data = buffer; // motor_pub.publish(&motor_msg); // // if (x==0.0){ // turn(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);} // } // } // } } 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() { // ros::Rate rate(10); motor_pwm[0]=0; motor_pwm[1]=0; motor_pwm[2]=0; motor_pwm[3]=0; nh.getHardware()->setBaud(BAUD); nh.initNode(); nh.advertise(pub); // nh.advertise(motor_pub); nh.subscribe(subtw); nh.subscribe(subr1); battery_thread.start(battery_pub); while (1) { nh.spinOnce(); if(nh.connected()) myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson else myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson } Thread::wait(10); }