LI SU / Mbed 2 deprecated Arm_Finger_Control

Arm_Finger_Control

Dependencies:   PID mbed ros_lib_indigo

main.cpp@0:4a5e692e02bf, 2018-08-22 (annotated)

Committer:
noname001
Date:
Wed Aug 22 08:11:10 2018 +0000
Revision:
0:4a5e692e02bf
ROS_FINGER;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
noname001 0:4a5e692e02bf 1 #include "mbed.h"
noname001 0:4a5e692e02bf 2 #include "PID.h"
noname001 0:4a5e692e02bf 3 #include <ros.h>
noname001 0:4a5e692e02bf 4 #include <std_msgs/Bool.h>
noname001 0:4a5e692e02bf 5 #include <std_msgs/Int32.h>
noname001 0:4a5e692e02bf 6 #include <geometry_msgs/Vector3.h>
noname001 0:4a5e692e02bf 7 PwmOut mypwm_left(D7);
noname001 0:4a5e692e02bf 8
noname001 0:4a5e692e02bf 9 DigitalOut myledg(D13); // IO used by pwm_io function
noname001 0:4a5e692e02bf 10 DigitalOut myledy(D14); // IO used by pwm_io function
noname001 0:4a5e692e02bf 11 DigitalOut myledr(D15); // IO used by pwm_io function
noname001 0:4a5e692e02bf 12
noname001 0:4a5e692e02bf 13 AnalogIn leftFSR(A0);
noname001 0:4a5e692e02bf 14 AnalogIn rightFSR(A3);
noname001 0:4a5e692e02bf 15 //gitalOut myled(LED1);
noname001 0:4a5e692e02bf 16 //DigitalOut myled(LED2);
noname001 0:4a5e692e02bf 17 Ticker timer1;
noname001 0:4a5e692e02bf 18 //Serial pc(SERIAL_TX, SERIAL_RX);
noname001 0:4a5e692e02bf 19
noname001 0:4a5e692e02bf 20
noname001 0:4a5e692e02bf 21
noname001 0:4a5e692e02bf 22 //int right_count = 1900;
noname001 0:4a5e692e02bf 23 //int init__right_PW = 1900;
noname001 0:4a5e692e02bf 24 int left_count = 1250;
noname001 0:4a5e692e02bf 25 int init__left_PW = 1250;
noname001 0:4a5e692e02bf 26
noname001 0:4a5e692e02bf 27 int count = 1250;
noname001 0:4a5e692e02bf 28 int init_PW=1250;
noname001 0:4a5e692e02bf 29
noname001 0:4a5e692e02bf 30 void init_TIMER(void);
noname001 0:4a5e692e02bf 31 void timer1_interrupt(void);
noname001 0:4a5e692e02bf 32
noname001 0:4a5e692e02bf 33 float meas_left;
noname001 0:4a5e692e02bf 34 float meas_right;
noname001 0:4a5e692e02bf 35
noname001 0:4a5e692e02bf 36
noname001 0:4a5e692e02bf 37 float T_read = 0;
noname001 0:4a5e692e02bf 38 float Td = 500.0;
noname001 0:4a5e692e02bf 39 float err = 0;
noname001 0:4a5e692e02bf 40 float err_old = 0;
noname001 0:4a5e692e02bf 41 float derivative = 0;
noname001 0:4a5e692e02bf 42 float dt = 0.002;
noname001 0:4a5e692e02bf 43 PID servo_pid(1.5, 1.0, 0.1, dt);
noname001 0:4a5e692e02bf 44 float PI_out = 0;
noname001 0:4a5e692e02bf 45 float PIDout = 0;
noname001 0:4a5e692e02bf 46 float Kp = 1.0;
noname001 0:4a5e692e02bf 47 float Kd = 1.0;
noname001 0:4a5e692e02bf 48 float Scale = 0.005;
noname001 0:4a5e692e02bf 49 int det = 0;
noname001 0:4a5e692e02bf 50 int count_light = 0;
noname001 0:4a5e692e02bf 51 int cmd_g = 0;
noname001 0:4a5e692e02bf 52 // ROS NODE
noname001 0:4a5e692e02bf 53 ros:: NodeHandle nh;
noname001 0:4a5e692e02bf 54
noname001 0:4a5e692e02bf 55 geometry_msgs::Vector3 fsr_msg;
noname001 0:4a5e692e02bf 56 ros::Publisher fsr("stm_msg", &fsr_msg);
noname001 0:4a5e692e02bf 57
noname001 0:4a5e692e02bf 58 void grip_callback(const std_msgs::Int32& msg)
noname001 0:4a5e692e02bf 59 {
noname001 0:4a5e692e02bf 60 //myled = 0;
noname001 0:4a5e692e02bf 61 std_msgs::Int32 command = msg;
noname001 0:4a5e692e02bf 62
noname001 0:4a5e692e02bf 63 det = command.data;
noname001 0:4a5e692e02bf 64
noname001 0:4a5e692e02bf 65
noname001 0:4a5e692e02bf 66 if (det==1400)
noname001 0:4a5e692e02bf 67 {
noname001 0:4a5e692e02bf 68 cmd_g = 1250;
noname001 0:4a5e692e02bf 69 //myled = 1;
noname001 0:4a5e692e02bf 70 while(cmd_g < 1400)
noname001 0:4a5e692e02bf 71 {
noname001 0:4a5e692e02bf 72 cmd_g = cmd_g + 5;
noname001 0:4a5e692e02bf 73 mypwm_left.pulsewidth_us(cmd_g);
noname001 0:4a5e692e02bf 74 }
noname001 0:4a5e692e02bf 75
noname001 0:4a5e692e02bf 76 }
noname001 0:4a5e692e02bf 77 else
noname001 0:4a5e692e02bf 78 {
noname001 0:4a5e692e02bf 79 mypwm_left.pulsewidth_us(init__left_PW);
noname001 0:4a5e692e02bf 80
noname001 0:4a5e692e02bf 81 }
noname001 0:4a5e692e02bf 82
noname001 0:4a5e692e02bf 83 }
noname001 0:4a5e692e02bf 84
noname001 0:4a5e692e02bf 85
noname001 0:4a5e692e02bf 86 ros::Subscriber<std_msgs::Int32> sub("gripper_command",grip_callback);
noname001 0:4a5e692e02bf 87
noname001 0:4a5e692e02bf 88
noname001 0:4a5e692e02bf 89 int main() {
noname001 0:4a5e692e02bf 90 // pc.printf("GOGOGO\n");
noname001 0:4a5e692e02bf 91
noname001 0:4a5e692e02bf 92 // pc.baud(9600);
noname001 0:4a5e692e02bf 93 mypwm_left.period_ms(20);
noname001 0:4a5e692e02bf 94 //mypwm_right.period_ms(20);
noname001 0:4a5e692e02bf 95 mypwm_left.pulsewidth_us(init__left_PW);
noname001 0:4a5e692e02bf 96 //mypwm_right.pulsewidth_us(init__right_PW );
noname001 0:4a5e692e02bf 97 //
noname001 0:4a5e692e02bf 98 //
noname001 0:4a5e692e02bf 99 // printf("pwm set to %.2f %%\n", mypwm.read() * 100);
noname001 0:4a5e692e02bf 100 //
noname001 0:4a5e692e02bf 101 nh.initNode();
noname001 0:4a5e692e02bf 102 nh.subscribe(sub);
noname001 0:4a5e692e02bf 103 nh.advertise(fsr);
noname001 0:4a5e692e02bf 104 init_TIMER();
noname001 0:4a5e692e02bf 105
noname001 0:4a5e692e02bf 106 while(1) {
noname001 0:4a5e692e02bf 107
noname001 0:4a5e692e02bf 108
noname001 0:4a5e692e02bf 109 if(det==1250)
noname001 0:4a5e692e02bf 110 {
noname001 0:4a5e692e02bf 111 fsr_msg.x = 0.0;
noname001 0:4a5e692e02bf 112 fsr_msg.y = T_read;
noname001 0:4a5e692e02bf 113 fsr_msg.z = left_count;
noname001 0:4a5e692e02bf 114 }
noname001 0:4a5e692e02bf 115 else if ( T_read > 500.0 || left_count <= 1400)
noname001 0:4a5e692e02bf 116 {
noname001 0:4a5e692e02bf 117 fsr_msg.x = 3;
noname001 0:4a5e692e02bf 118 fsr_msg.y = T_read;
noname001 0:4a5e692e02bf 119 fsr_msg.z = left_count;
noname001 0:4a5e692e02bf 120 }
noname001 0:4a5e692e02bf 121
noname001 0:4a5e692e02bf 122 count_light = count_light + 1;
noname001 0:4a5e692e02bf 123 if(T_read > 180.0)
noname001 0:4a5e692e02bf 124 {
noname001 0:4a5e692e02bf 125 myledg = 1;
noname001 0:4a5e692e02bf 126 count_light = 0;
noname001 0:4a5e692e02bf 127 }
noname001 0:4a5e692e02bf 128 else if(count_light<5)
noname001 0:4a5e692e02bf 129 {
noname001 0:4a5e692e02bf 130 myledg = 1;
noname001 0:4a5e692e02bf 131 }
noname001 0:4a5e692e02bf 132 else
noname001 0:4a5e692e02bf 133 {
noname001 0:4a5e692e02bf 134 myledg = 0;
noname001 0:4a5e692e02bf 135 }
noname001 0:4a5e692e02bf 136
noname001 0:4a5e692e02bf 137 fsr.publish( &fsr_msg);
noname001 0:4a5e692e02bf 138
noname001 0:4a5e692e02bf 139 nh.spinOnce();
noname001 0:4a5e692e02bf 140 wait_ms(1000);
noname001 0:4a5e692e02bf 141
noname001 0:4a5e692e02bf 142
noname001 0:4a5e692e02bf 143 //pc.printf("pwm set to %d %%\n", init_PW);
noname001 0:4a5e692e02bf 144
noname001 0:4a5e692e02bf 145
noname001 0:4a5e692e02bf 146 }
noname001 0:4a5e692e02bf 147 }
noname001 0:4a5e692e02bf 148
noname001 0:4a5e692e02bf 149 void init_TIMER(){
noname001 0:4a5e692e02bf 150
noname001 0:4a5e692e02bf 151 timer1.attach_us(&timer1_interrupt, 10000.0);
noname001 0:4a5e692e02bf 152 }
noname001 0:4a5e692e02bf 153
noname001 0:4a5e692e02bf 154 void timer1_interrupt(){
noname001 0:4a5e692e02bf 155
noname001 0:4a5e692e02bf 156 meas_left = leftFSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noname001 0:4a5e692e02bf 157 meas_left = meas_left * 1000; // Change the value to be in the 0 to 3300 range
noname001 0:4a5e692e02bf 158
noname001 0:4a5e692e02bf 159 meas_right = rightFSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noname001 0:4a5e692e02bf 160 meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range
noname001 0:4a5e692e02bf 161 //printf("%.0f\t%.0f\r", meas_left, meas_right);
noname001 0:4a5e692e02bf 162
noname001 0:4a5e692e02bf 163 if(meas_right>meas_left)
noname001 0:4a5e692e02bf 164 {
noname001 0:4a5e692e02bf 165 T_read = meas_right;
noname001 0:4a5e692e02bf 166 }
noname001 0:4a5e692e02bf 167 else
noname001 0:4a5e692e02bf 168 {
noname001 0:4a5e692e02bf 169 T_read = meas_left;
noname001 0:4a5e692e02bf 170 }
noname001 0:4a5e692e02bf 171 //printf("%.0f\t%.0f\t%.0f\r", meas_left, meas_right,T_read);
noname001 0:4a5e692e02bf 172
noname001 0:4a5e692e02bf 173 err = Td - T_read;
noname001 0:4a5e692e02bf 174 derivative = ( err - err_old ) / dt;
noname001 0:4a5e692e02bf 175 err_old = err;
noname001 0:4a5e692e02bf 176
noname001 0:4a5e692e02bf 177
noname001 0:4a5e692e02bf 178
noname001 0:4a5e692e02bf 179 //PI_out = Kp * err + Kd * derivative;
noname001 0:4a5e692e02bf 180 //right_count = right_count - int(Scale * PI_out);
noname001 0:4a5e692e02bf 181
noname001 0:4a5e692e02bf 182
noname001 0:4a5e692e02bf 183
noname001 0:4a5e692e02bf 184
noname001 0:4a5e692e02bf 185
noname001 0:4a5e692e02bf 186 servo_pid.Compute(Td, T_read);
noname001 0:4a5e692e02bf 187 PIDout = servo_pid.output;
noname001 0:4a5e692e02bf 188 left_count = left_count + int(0.002 * PIDout);
noname001 0:4a5e692e02bf 189
noname001 0:4a5e692e02bf 190 // printf("%d \n", left_count);
noname001 0:4a5e692e02bf 191
noname001 0:4a5e692e02bf 192 //printf("%.0f \r", meas_right);
noname001 0:4a5e692e02bf 193 ///// left_count = left_count - 1;
noname001 0:4a5e692e02bf 194 //right_count = right_count - 10;
noname001 0:4a5e692e02bf 195
noname001 0:4a5e692e02bf 196
noname001 0:4a5e692e02bf 197 if(left_count > 1400) //1700
noname001 0:4a5e692e02bf 198 {
noname001 0:4a5e692e02bf 199 left_count = 1400; //1850
noname001 0:4a5e692e02bf 200 }
noname001 0:4a5e692e02bf 201
noname001 0:4a5e692e02bf 202 /*if(right_count < 1750) //1700
noname001 0:4a5e692e02bf 203 {
noname001 0:4a5e692e02bf 204 right_count = 2000; //1850
noname001 0:4a5e692e02bf 205 }
noname001 0:4a5e692e02bf 206 */
noname001 0:4a5e692e02bf 207
noname001 0:4a5e692e02bf 208
noname001 0:4a5e692e02bf 209 //mypwm_right.pulsewidth_us(left_count);
noname001 0:4a5e692e02bf 210 //pc.printf("pwm set to %d %%\n", left_count);
noname001 0:4a5e692e02bf 211 //pc.printf("pwm set to %d %%\n", right_count);
noname001 0:4a5e692e02bf 212
noname001 0:4a5e692e02bf 213 }