John Lam / Mbed 2 deprecated fyp

fyp base control

Dependencies:   mbed mbed-rtos ros_lib_melodic_

main.cpp@1:782534ae7166, 24 months ago (annotated)

Committer:
howanglam3
Date:
Sat Jul 30 18:05:58 2022 +0000
Revision:
1:782534ae7166
Parent:
0:54452c8078db
Child:
3:5402a080282c
noetttic;

Who changed what in which revision?

UserRevisionLine numberNew 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 1:782534ae7166 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 1:782534ae7166 17 Thread battery_thread;
howanglam3 1:782534ae7166 18
howanglam3 1:782534ae7166 19 AnalogIn battery(A0);
howanglam3 0:54452c8078db 20
howanglam3 0:54452c8078db 21 DigitalOut myled = LED1;
howanglam3 0:54452c8078db 22 // -> Motor PWN Pin
howanglam3 1:782534ae7166 23 PwmOut motor_pwm[] = {D6, D5, D3, D4};
howanglam3 0:54452c8078db 24 DigitalOut LED01 = D7;
howanglam3 0:54452c8078db 25 DigitalOut LED02 = D8;
howanglam3 0:54452c8078db 26 DigitalOut LED03 = D9;
howanglam3 0:54452c8078db 27 //motor number
howanglam3 0:54452c8078db 28
howanglam3 1:782534ae7166 29 void turn(float r, float l){
howanglam3 1:782534ae7166 30 float a, b, c, d;
howanglam3 1:782534ae7166 31 if (l < 0){
howanglam3 1:782534ae7166 32 a = 0.0;
howanglam3 1:782534ae7166 33 b = -l;
howanglam3 1:782534ae7166 34 }
howanglam3 1:782534ae7166 35 else{
howanglam3 1:782534ae7166 36 a = l;
howanglam3 1:782534ae7166 37 b = 0.0;
howanglam3 1:782534ae7166 38 }
howanglam3 1:782534ae7166 39 if (r < 0){
howanglam3 1:782534ae7166 40 c = 0.0;
howanglam3 1:782534ae7166 41 d = -r;
howanglam3 1:782534ae7166 42 }
howanglam3 1:782534ae7166 43 else{
howanglam3 1:782534ae7166 44 c = r;
howanglam3 1:782534ae7166 45 d = 0.0;
howanglam3 1:782534ae7166 46 }
howanglam3 1:782534ae7166 47 motor_pwm[0]=a;
howanglam3 1:782534ae7166 48 motor_pwm[1]=b;
howanglam3 1:782534ae7166 49 motor_pwm[2]=c;
howanglam3 1:782534ae7166 50 motor_pwm[3]=d;
howanglam3 0:54452c8078db 51 }
howanglam3 0:54452c8078db 52
howanglam3 1:782534ae7166 53 void resize(float &a, float &b){
howanglam3 1:782534ae7166 54 if (a != 0 or b != 0) {
howanglam3 1:782534ae7166 55 if (b > a){
howanglam3 1:782534ae7166 56 a = a/b;
howanglam3 1:782534ae7166 57 if (b > 1)
howanglam3 1:782534ae7166 58 b = 1;
howanglam3 1:782534ae7166 59 }
howanglam3 1:782534ae7166 60 else{
howanglam3 1:782534ae7166 61 b = b/a;
howanglam3 1:782534ae7166 62 if (a > 1)
howanglam3 1:782534ae7166 63 a = 1;
howanglam3 1:782534ae7166 64 }
howanglam3 1:782534ae7166 65 }
howanglam3 1:782534ae7166 66
howanglam3 0:54452c8078db 67 }
howanglam3 1:782534ae7166 68
howanglam3 0:54452c8078db 69 void r1Callback(const Bool& _msg){
howanglam3 0:54452c8078db 70 bool check = _msg.data;
howanglam3 0:54452c8078db 71 if(check){
howanglam3 0:54452c8078db 72 LED01 = 1;
howanglam3 0:54452c8078db 73 LED02 = 1;
howanglam3 0:54452c8078db 74 LED03 = 1;}
howanglam3 0:54452c8078db 75 else{
howanglam3 0:54452c8078db 76 LED01 = 0;
howanglam3 0:54452c8078db 77 LED02 = 0;
howanglam3 0:54452c8078db 78 LED03 = 0;}
howanglam3 0:54452c8078db 79 }
howanglam3 0:54452c8078db 80
howanglam3 1:782534ae7166 81 float sum = 0.0;
howanglam3 1:782534ae7166 82 //std_msgs::String motor_msg;
howanglam3 1:782534ae7166 83 //char buffer[50];
howanglam3 1:782534ae7166 84 //Publisher motor_pub("motor", &motor_msg);
howanglam3 1:782534ae7166 85
howanglam3 1:782534ae7166 86
howanglam3 0:54452c8078db 87 void twCallback(const Twist& _msg) {
howanglam3 0:54452c8078db 88 float x = _msg.linear.x;
howanglam3 0:54452c8078db 89 float y = _msg.linear.y;
howanglam3 1:782534ae7166 90 sum =abs(x)+abs(y);
howanglam3 1:782534ae7166 91 resize(y, sum);
howanglam3 1:782534ae7166 92 if (y < 0)
howanglam3 1:782534ae7166 93 sum = -sum;
howanglam3 1:782534ae7166 94 if (x < 0)
howanglam3 1:782534ae7166 95 turn(y, sum);
howanglam3 1:782534ae7166 96 else
howanglam3 1:782534ae7166 97 turn(sum, y);
howanglam3 1:782534ae7166 98
howanglam3 1:782534ae7166 99 // sprintf(buffer, "turn: %f, %f", sum, y);
howanglam3 1:782534ae7166 100 // motor_msg.data = buffer;
howanglam3 1:782534ae7166 101 // motor_pub.publish(&motor_msg);
howanglam3 0:54452c8078db 102
howanglam3 1:782534ae7166 103 //
howanglam3 1:782534ae7166 104 // if (x==0.0){
howanglam3 1:782534ae7166 105 // turn(y,y);
howanglam3 0:54452c8078db 106 // }
howanglam3 1:782534ae7166 107 // else{
howanglam3 1:782534ae7166 108 // float angle = atan(abs(y)/abs(x));
howanglam3 1:782534ae7166 109 // float angle45 = atan(1.0);
howanglam3 1:782534ae7166 110 // if(x<0){
howanglam3 1:782534ae7166 111 // if(angle <= angle45){
howanglam3 1:782534ae7166 112 // motor_pwm[0]=0;
howanglam3 1:782534ae7166 113 // motor_pwm[1]=abs(x + abs(y));
howanglam3 1:782534ae7166 114 // motor_pwm[2]=-x;
howanglam3 1:782534ae7166 115 // motor_pwm[3]=0;
howanglam3 1:782534ae7166 116 //
howanglam3 1:782534ae7166 117 // }
howanglam3 1:782534ae7166 118 // else{
howanglam3 1:782534ae7166 119 // if(y<0){
howanglam3 1:782534ae7166 120 // antiturning(-y, -y+x);}
howanglam3 1:782534ae7166 121 // else{
howanglam3 1:782534ae7166 122 // turning(y+x, y);}
howanglam3 1:782534ae7166 123 // }
howanglam3 1:782534ae7166 124 // }
howanglam3 1:782534ae7166 125 // else {
howanglam3 1:782534ae7166 126 // if(angle <= angle45){
howanglam3 1:782534ae7166 127 // motor_pwm[0]=x;
howanglam3 1:782534ae7166 128 // motor_pwm[1]=0;
howanglam3 1:782534ae7166 129 // motor_pwm[2]=0;
howanglam3 1:782534ae7166 130 // motor_pwm[3]=x - abs(y);
howanglam3 1:782534ae7166 131 //
howanglam3 1:782534ae7166 132 // }
howanglam3 1:782534ae7166 133 // else{
howanglam3 1:782534ae7166 134 // if(y<0){
howanglam3 1:782534ae7166 135 // antiturning(-y-x,-y);}
howanglam3 1:782534ae7166 136 // else{
howanglam3 1:782534ae7166 137 // turning(y, y-x);}
howanglam3 1:782534ae7166 138 // }
howanglam3 1:782534ae7166 139 // }
howanglam3 0:54452c8078db 140 // }
howanglam3 0:54452c8078db 141
howanglam3 0:54452c8078db 142 }
howanglam3 1:782534ae7166 143 std_msgs::String str_msg;
howanglam3 1:782534ae7166 144 Publisher pub("battery", &str_msg);
howanglam3 0:54452c8078db 145
howanglam3 0:54452c8078db 146 Subscriber<Twist> subtw("base_twist", &twCallback);
howanglam3 0:54452c8078db 147 Subscriber<Bool> subr1("bt_r1", &r1Callback);
howanglam3 1:782534ae7166 148
howanglam3 1:782534ae7166 149 void battery_pub(){
howanglam3 1:782534ae7166 150 char buf[10];
howanglam3 1:782534ae7166 151 while(1){
howanglam3 1:782534ae7166 152
howanglam3 1:782534ae7166 153 sprintf(buf, "%d", int(((battery.read()*3.3f) - (6/2.68))/(2.4/268)));
howanglam3 1:782534ae7166 154 // sprintf(buf,"%f",battery.read());
howanglam3 1:782534ae7166 155 str_msg.data = buf;
howanglam3 1:782534ae7166 156 pub.publish(&str_msg);
howanglam3 1:782534ae7166 157 // nh.spinOnce();
howanglam3 1:782534ae7166 158 Thread::wait(1000);
howanglam3 1:782534ae7166 159
howanglam3 1:782534ae7166 160 }
howanglam3 1:782534ae7166 161 }
howanglam3 1:782534ae7166 162
howanglam3 0:54452c8078db 163 int main() {
howanglam3 0:54452c8078db 164 //Rate rate(10);
howanglam3 0:54452c8078db 165 nh.getHardware()->setBaud(BAUD);
howanglam3 0:54452c8078db 166 nh.initNode();
howanglam3 1:782534ae7166 167 nh.advertise(pub);
howanglam3 1:782534ae7166 168 // nh.advertise(motor_pub);
howanglam3 0:54452c8078db 169 nh.subscribe(subtw);
howanglam3 0:54452c8078db 170 nh.subscribe(subr1);
howanglam3 0:54452c8078db 171
howanglam3 0:54452c8078db 172 motor_pwm[0]=0;
howanglam3 0:54452c8078db 173 motor_pwm[1]=0;
howanglam3 0:54452c8078db 174 motor_pwm[2]=0;
howanglam3 0:54452c8078db 175 motor_pwm[3]=0;
howanglam3 1:782534ae7166 176
howanglam3 1:782534ae7166 177 battery_thread.start(battery_pub);
howanglam3 0:54452c8078db 178 while (1) {
howanglam3 0:54452c8078db 179 nh.spinOnce();
howanglam3 0:54452c8078db 180
howanglam3 0:54452c8078db 181
howanglam3 0:54452c8078db 182 if(nh.connected())
howanglam3 0:54452c8078db 183 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
howanglam3 0:54452c8078db 184 else
howanglam3 0:54452c8078db 185 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
howanglam3 0:54452c8078db 186 }
howanglam3 0:54452c8078db 187
howanglam3 0:54452c8078db 188 }