![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fyp base control
Dependencies: mbed mbed-rtos ros_lib_melodic_
main.cpp@0:54452c8078db, 24 months ago (annotated)
- Committer:
- howanglam3
- Date:
- Fri Jul 29 14:42:35 2022 +0000
- Revision:
- 0:54452c8078db
- Child:
- 1:782534ae7166
non rtos
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
howanglam3 | 0:54452c8078db | 1 | #define BAUD 115200 |
howanglam3 | 0:54452c8078db | 2 | #include "mbed.h" |
howanglam3 | 0:54452c8078db | 3 | #include "math.h" |
howanglam3 | 0:54452c8078db | 4 | //#include "rtos.h" |
howanglam3 | 0:54452c8078db | 5 | #include <ros.h> |
howanglam3 | 0:54452c8078db | 6 | #include "std_msgs/Bool.h" |
howanglam3 | 0:54452c8078db | 7 | #include "std_msgs/String.h" |
howanglam3 | 0:54452c8078db | 8 | #include "geometry_msgs/Twist.h" |
howanglam3 | 0:54452c8078db | 9 | |
howanglam3 | 0:54452c8078db | 10 | |
howanglam3 | 0:54452c8078db | 11 | using namespace ros; |
howanglam3 | 0:54452c8078db | 12 | using namespace std_msgs; |
howanglam3 | 0:54452c8078db | 13 | using namespace geometry_msgs; |
howanglam3 | 0:54452c8078db | 14 | |
howanglam3 | 0:54452c8078db | 15 | |
howanglam3 | 0:54452c8078db | 16 | NodeHandle nh; |
howanglam3 | 0:54452c8078db | 17 | |
howanglam3 | 0:54452c8078db | 18 | DigitalOut myled = LED1; |
howanglam3 | 0:54452c8078db | 19 | // -> Motor PWN Pin |
howanglam3 | 0:54452c8078db | 20 | PwmOut motor_pwm[] = {D6, D5, D4, D3}; |
howanglam3 | 0:54452c8078db | 21 | DigitalOut LED01 = D7; |
howanglam3 | 0:54452c8078db | 22 | DigitalOut LED02 = D8; |
howanglam3 | 0:54452c8078db | 23 | DigitalOut LED03 = D9; |
howanglam3 | 0:54452c8078db | 24 | //motor number |
howanglam3 | 0:54452c8078db | 25 | |
howanglam3 | 0:54452c8078db | 26 | void antiturning(float a, float b){ |
howanglam3 | 0:54452c8078db | 27 | motor_pwm[0]=0; |
howanglam3 | 0:54452c8078db | 28 | motor_pwm[1]=a; |
howanglam3 | 0:54452c8078db | 29 | motor_pwm[2]=0; |
howanglam3 | 0:54452c8078db | 30 | motor_pwm[3]=b; |
howanglam3 | 0:54452c8078db | 31 | } |
howanglam3 | 0:54452c8078db | 32 | |
howanglam3 | 0:54452c8078db | 33 | void turning(float a, float b){ |
howanglam3 | 0:54452c8078db | 34 | motor_pwm[0]=a; |
howanglam3 | 0:54452c8078db | 35 | motor_pwm[1]=0; |
howanglam3 | 0:54452c8078db | 36 | motor_pwm[2]=b; |
howanglam3 | 0:54452c8078db | 37 | motor_pwm[3]=0; |
howanglam3 | 0:54452c8078db | 38 | } |
howanglam3 | 0:54452c8078db | 39 | |
howanglam3 | 0:54452c8078db | 40 | void r1Callback(const Bool& _msg){ |
howanglam3 | 0:54452c8078db | 41 | bool check = _msg.data; |
howanglam3 | 0:54452c8078db | 42 | if(check){ |
howanglam3 | 0:54452c8078db | 43 | LED01 = 1; |
howanglam3 | 0:54452c8078db | 44 | LED02 = 1; |
howanglam3 | 0:54452c8078db | 45 | LED03 = 1;} |
howanglam3 | 0:54452c8078db | 46 | else{ |
howanglam3 | 0:54452c8078db | 47 | LED01 = 0; |
howanglam3 | 0:54452c8078db | 48 | LED02 = 0; |
howanglam3 | 0:54452c8078db | 49 | LED03 = 0;} |
howanglam3 | 0:54452c8078db | 50 | } |
howanglam3 | 0:54452c8078db | 51 | |
howanglam3 | 0:54452c8078db | 52 | void twCallback(const Twist& _msg) { |
howanglam3 | 0:54452c8078db | 53 | float x = _msg.linear.x; |
howanglam3 | 0:54452c8078db | 54 | float y = _msg.linear.y; |
howanglam3 | 0:54452c8078db | 55 | |
howanglam3 | 0:54452c8078db | 56 | if (x==0.0){ |
howanglam3 | 0:54452c8078db | 57 | if(y<0){ |
howanglam3 | 0:54452c8078db | 58 | antiturning(abs(y), abs(y)); |
howanglam3 | 0:54452c8078db | 59 | } |
howanglam3 | 0:54452c8078db | 60 | else{ |
howanglam3 | 0:54452c8078db | 61 | turning(y, y); |
howanglam3 | 0:54452c8078db | 62 | } |
howanglam3 | 0:54452c8078db | 63 | } |
howanglam3 | 0:54452c8078db | 64 | else{ |
howanglam3 | 0:54452c8078db | 65 | float angle = atan(abs(y)/abs(x)); |
howanglam3 | 0:54452c8078db | 66 | float angle45 = atan(1.0); |
howanglam3 | 0:54452c8078db | 67 | if(x<0){ |
howanglam3 | 0:54452c8078db | 68 | if(angle <= angle45){ |
howanglam3 | 0:54452c8078db | 69 | motor_pwm[0]=0; |
howanglam3 | 0:54452c8078db | 70 | motor_pwm[1]=abs(x + abs(y)); |
howanglam3 | 0:54452c8078db | 71 | motor_pwm[2]=-x; |
howanglam3 | 0:54452c8078db | 72 | motor_pwm[3]=0; |
howanglam3 | 0:54452c8078db | 73 | |
howanglam3 | 0:54452c8078db | 74 | } |
howanglam3 | 0:54452c8078db | 75 | else{ |
howanglam3 | 0:54452c8078db | 76 | if(y<0){ |
howanglam3 | 0:54452c8078db | 77 | antiturning(-y, -y+x);} |
howanglam3 | 0:54452c8078db | 78 | else{ |
howanglam3 | 0:54452c8078db | 79 | turning(y+x, y);} |
howanglam3 | 0:54452c8078db | 80 | } |
howanglam3 | 0:54452c8078db | 81 | } |
howanglam3 | 0:54452c8078db | 82 | else { |
howanglam3 | 0:54452c8078db | 83 | if(angle <= angle45){ |
howanglam3 | 0:54452c8078db | 84 | motor_pwm[0]=x; |
howanglam3 | 0:54452c8078db | 85 | motor_pwm[1]=0; |
howanglam3 | 0:54452c8078db | 86 | motor_pwm[2]=0; |
howanglam3 | 0:54452c8078db | 87 | motor_pwm[3]=x - abs(y); |
howanglam3 | 0:54452c8078db | 88 | |
howanglam3 | 0:54452c8078db | 89 | } |
howanglam3 | 0:54452c8078db | 90 | else{ |
howanglam3 | 0:54452c8078db | 91 | if(y<0){ |
howanglam3 | 0:54452c8078db | 92 | antiturning(-y-x,-y);} |
howanglam3 | 0:54452c8078db | 93 | else{ |
howanglam3 | 0:54452c8078db | 94 | turning(y, y-x);} |
howanglam3 | 0:54452c8078db | 95 | } |
howanglam3 | 0:54452c8078db | 96 | } |
howanglam3 | 0:54452c8078db | 97 | } |
howanglam3 | 0:54452c8078db | 98 | // if(y<0) |
howanglam3 | 0:54452c8078db | 99 | // { |
howanglam3 | 0:54452c8078db | 100 | // motor_pwm[2]=0; |
howanglam3 | 0:54452c8078db | 101 | // motor_pwm[3]=abs(y); |
howanglam3 | 0:54452c8078db | 102 | // } |
howanglam3 | 0:54452c8078db | 103 | // else |
howanglam3 | 0:54452c8078db | 104 | // { |
howanglam3 | 0:54452c8078db | 105 | // motor_pwm[2]=y; |
howanglam3 | 0:54452c8078db | 106 | // motor_pwm[3]=0; |
howanglam3 | 0:54452c8078db | 107 | // } |
howanglam3 | 0:54452c8078db | 108 | // if(x<0) |
howanglam3 | 0:54452c8078db | 109 | // { |
howanglam3 | 0:54452c8078db | 110 | // motor_pwm[0]=0; |
howanglam3 | 0:54452c8078db | 111 | // motor_pwm[1]=abs(x); |
howanglam3 | 0:54452c8078db | 112 | // |
howanglam3 | 0:54452c8078db | 113 | // } |
howanglam3 | 0:54452c8078db | 114 | // else |
howanglam3 | 0:54452c8078db | 115 | // { |
howanglam3 | 0:54452c8078db | 116 | // motor_pwm[0]=x; |
howanglam3 | 0:54452c8078db | 117 | // motor_pwm[1]=0; |
howanglam3 | 0:54452c8078db | 118 | // } |
howanglam3 | 0:54452c8078db | 119 | |
howanglam3 | 0:54452c8078db | 120 | } |
howanglam3 | 0:54452c8078db | 121 | |
howanglam3 | 0:54452c8078db | 122 | Subscriber<Twist> subtw("base_twist", &twCallback); |
howanglam3 | 0:54452c8078db | 123 | Subscriber<Bool> subr1("bt_r1", &r1Callback); |
howanglam3 | 0:54452c8078db | 124 | int main() { |
howanglam3 | 0:54452c8078db | 125 | //Rate rate(10); |
howanglam3 | 0:54452c8078db | 126 | nh.getHardware()->setBaud(BAUD); |
howanglam3 | 0:54452c8078db | 127 | nh.initNode(); |
howanglam3 | 0:54452c8078db | 128 | |
howanglam3 | 0:54452c8078db | 129 | nh.subscribe(subtw); |
howanglam3 | 0:54452c8078db | 130 | nh.subscribe(subr1); |
howanglam3 | 0:54452c8078db | 131 | |
howanglam3 | 0:54452c8078db | 132 | motor_pwm[0]=0; |
howanglam3 | 0:54452c8078db | 133 | motor_pwm[1]=0; |
howanglam3 | 0:54452c8078db | 134 | motor_pwm[2]=0; |
howanglam3 | 0:54452c8078db | 135 | motor_pwm[3]=0; |
howanglam3 | 0:54452c8078db | 136 | |
howanglam3 | 0:54452c8078db | 137 | while (1) { |
howanglam3 | 0:54452c8078db | 138 | nh.spinOnce(); |
howanglam3 | 0:54452c8078db | 139 | |
howanglam3 | 0:54452c8078db | 140 | |
howanglam3 | 0:54452c8078db | 141 | if(nh.connected()) |
howanglam3 | 0:54452c8078db | 142 | myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson |
howanglam3 | 0:54452c8078db | 143 | else |
howanglam3 | 0:54452c8078db | 144 | myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson |
howanglam3 | 0:54452c8078db | 145 | } |
howanglam3 | 0:54452c8078db | 146 | |
howanglam3 | 0:54452c8078db | 147 | } |