John Lam / Mbed 2 deprecated fyp

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?

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 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 }