WRS2019
Dependencies: mbed BufferedSerial PID2 JY901 ros_lib_kinetic TextLCD i2cmaster Make_Sequencer_3
main.cpp@6:55e60b9d7ff1, 2019-12-18 (annotated)
- Committer:
- sgrsn
- Date:
- Wed Dec 18 02:59:04 2019 +0000
- Branch:
- StartFromROS
- Revision:
- 6:55e60b9d7ff1
- Parent:
- 5:a5dc3090ba44
Fix that BuffredSerial be in ros_lib_kinetic.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:f1459eec7228 | 1 | #include "mbed.h" |
sgrsn | 0:f1459eec7228 | 2 | #include <string> |
sgrsn | 0:f1459eec7228 | 3 | #include "i2cmaster.h" |
sgrsn | 0:f1459eec7228 | 4 | #include "JY901.h" |
sgrsn | 0:f1459eec7228 | 5 | #include "PID.h" |
sgrsn | 0:f1459eec7228 | 6 | #include "MakeSequencer.h" |
sgrsn | 0:f1459eec7228 | 7 | #include "define.h" |
sgrsn | 0:f1459eec7228 | 8 | |
sgrsn | 4:25ab7216447f | 9 | #include <ros.h> |
sgrsn | 4:25ab7216447f | 10 | #include <std_msgs/Empty.h> |
sgrsn | 4:25ab7216447f | 11 | |
sgrsn | 0:f1459eec7228 | 12 | #include "TextLCD.h" |
sgrsn | 0:f1459eec7228 | 13 | |
sgrsn | 4:25ab7216447f | 14 | // robot start when startable is true |
sgrsn | 4:25ab7216447f | 15 | bool startable = false; |
sgrsn | 4:25ab7216447f | 16 | |
sgrsn | 4:25ab7216447f | 17 | // for ROS |
sgrsn | 4:25ab7216447f | 18 | ros::NodeHandle nh; |
sgrsn | 4:25ab7216447f | 19 | void messageCb(const std_msgs::Empty& toggle_msg){ |
sgrsn | 4:25ab7216447f | 20 | startable = true; |
sgrsn | 4:25ab7216447f | 21 | } |
sgrsn | 4:25ab7216447f | 22 | ros::Subscriber<std_msgs::Empty> sub("/robot/start", &messageCb); |
sgrsn | 0:f1459eec7228 | 23 | |
sgrsn | 0:f1459eec7228 | 24 | // MakeSequencer |
sgrsn | 0:f1459eec7228 | 25 | #define SIZE 6 |
sgrsn | 0:f1459eec7228 | 26 | #define ArraySize(array) (sizeof(array) / sizeof(array[0])) |
sgrsn | 0:f1459eec7228 | 27 | |
sgrsn | 5:a5dc3090ba44 | 28 | // Robot Parameter |
sgrsn | 1:f102831401a8 | 29 | float wheel_d = 127; // メカナム直径[mm] |
sgrsn | 1:f102831401a8 | 30 | float wheel_r = 63.5; |
sgrsn | 1:f102831401a8 | 31 | float deg_per_pulse = 0.0072; // ステッピングモータ(AR46SAK-PS50-1) |
sgrsn | 0:f1459eec7228 | 32 | |
sgrsn | 1:f102831401a8 | 33 | float DEG_TO_RAD = PI/180.0; |
sgrsn | 1:f102831401a8 | 34 | float RAD_TO_DEG = 180.0/PI; |
sgrsn | 1:f102831401a8 | 35 | |
sgrsn | 5:a5dc3090ba44 | 36 | // Prototype |
sgrsn | 1:f102831401a8 | 37 | int controlMotor(int ch, int frequency); |
sgrsn | 0:f1459eec7228 | 38 | void coastAllMotor(); |
sgrsn | 1:f102831401a8 | 39 | |
sgrsn | 5:a5dc3090ba44 | 40 | // this class Count Stepping Motor Pulse for wheel |
sgrsn | 1:f102831401a8 | 41 | class CountWheel |
sgrsn | 1:f102831401a8 | 42 | { |
sgrsn | 1:f102831401a8 | 43 | public: |
sgrsn | 1:f102831401a8 | 44 | CountWheel(Timer *t) |
sgrsn | 1:f102831401a8 | 45 | { |
sgrsn | 1:f102831401a8 | 46 | _t = t; |
sgrsn | 1:f102831401a8 | 47 | _t -> start(); |
sgrsn | 1:f102831401a8 | 48 | } |
sgrsn | 1:f102831401a8 | 49 | float getRadian(float frequency) |
sgrsn | 1:f102831401a8 | 50 | { |
sgrsn | 1:f102831401a8 | 51 | last_time = current_time; |
sgrsn | 1:f102831401a8 | 52 | current_time = _t -> read(); |
sgrsn | 1:f102831401a8 | 53 | float dt = current_time - last_time; |
sgrsn | 1:f102831401a8 | 54 | float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 55 | return delta_rad; |
sgrsn | 1:f102831401a8 | 56 | } |
sgrsn | 1:f102831401a8 | 57 | |
sgrsn | 1:f102831401a8 | 58 | private: |
sgrsn | 1:f102831401a8 | 59 | Timer *_t; |
sgrsn | 1:f102831401a8 | 60 | float last_time; |
sgrsn | 1:f102831401a8 | 61 | float current_time; |
sgrsn | 1:f102831401a8 | 62 | }; |
sgrsn | 1:f102831401a8 | 63 | |
sgrsn | 5:a5dc3090ba44 | 64 | // this class make Robot Path to complement target value linearly |
sgrsn | 1:f102831401a8 | 65 | class MakePath |
sgrsn | 1:f102831401a8 | 66 | { |
sgrsn | 1:f102831401a8 | 67 | public: |
sgrsn | 1:f102831401a8 | 68 | MakePath() |
sgrsn | 1:f102831401a8 | 69 | { |
sgrsn | 1:f102831401a8 | 70 | } |
sgrsn | 1:f102831401a8 | 71 | int makePath(int targetX, int targetY, int targetZ, int x, int y, int z) |
sgrsn | 1:f102831401a8 | 72 | { |
sgrsn | 1:f102831401a8 | 73 | int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0; //5mm間隔 |
sgrsn | 1:f102831401a8 | 74 | //printf("num = %d\r\n", num); |
sgrsn | 1:f102831401a8 | 75 | for(int i = 1; i <= num; i++) |
sgrsn | 1:f102831401a8 | 76 | { |
sgrsn | 1:f102831401a8 | 77 | float a = float(i) / num; |
sgrsn | 1:f102831401a8 | 78 | PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); |
sgrsn | 1:f102831401a8 | 79 | PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); |
sgrsn | 1:f102831401a8 | 80 | PATH[i-1][2] = z + float(targetZ - z) * a; |
sgrsn | 1:f102831401a8 | 81 | } |
sgrsn | 1:f102831401a8 | 82 | if(num > 0) |
sgrsn | 1:f102831401a8 | 83 | { |
sgrsn | 1:f102831401a8 | 84 | PATH[num-1][0] = targetX; |
sgrsn | 1:f102831401a8 | 85 | PATH[num-1][1] = targetY; |
sgrsn | 1:f102831401a8 | 86 | PATH[num-1][2] = targetZ; |
sgrsn | 1:f102831401a8 | 87 | } |
sgrsn | 1:f102831401a8 | 88 | else |
sgrsn | 1:f102831401a8 | 89 | { |
sgrsn | 1:f102831401a8 | 90 | PATH[0][0] = targetX; |
sgrsn | 1:f102831401a8 | 91 | PATH[0][1] = targetY; |
sgrsn | 1:f102831401a8 | 92 | PATH[0][2] = targetZ; |
sgrsn | 1:f102831401a8 | 93 | num = 1; |
sgrsn | 1:f102831401a8 | 94 | } |
sgrsn | 1:f102831401a8 | 95 | return num; |
sgrsn | 1:f102831401a8 | 96 | } |
sgrsn | 1:f102831401a8 | 97 | |
sgrsn | 1:f102831401a8 | 98 | int getPathX(int i) |
sgrsn | 1:f102831401a8 | 99 | { |
sgrsn | 1:f102831401a8 | 100 | return PATH[i][0]; |
sgrsn | 1:f102831401a8 | 101 | } |
sgrsn | 1:f102831401a8 | 102 | int getPathY(int i) |
sgrsn | 1:f102831401a8 | 103 | { |
sgrsn | 1:f102831401a8 | 104 | return PATH[i][1]; |
sgrsn | 1:f102831401a8 | 105 | } |
sgrsn | 1:f102831401a8 | 106 | int getPathZ(int i) |
sgrsn | 1:f102831401a8 | 107 | { |
sgrsn | 1:f102831401a8 | 108 | return PATH[i][2]; |
sgrsn | 1:f102831401a8 | 109 | } |
sgrsn | 1:f102831401a8 | 110 | |
sgrsn | 1:f102831401a8 | 111 | private: |
sgrsn | 1:f102831401a8 | 112 | int PATH[500][3]; |
sgrsn | 1:f102831401a8 | 113 | }; |
sgrsn | 0:f1459eec7228 | 114 | |
sgrsn | 5:a5dc3090ba44 | 115 | // IIC address |
sgrsn | 0:f1459eec7228 | 116 | int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; |
sgrsn | 0:f1459eec7228 | 117 | int Register[0x20] = {}; |
sgrsn | 0:f1459eec7228 | 118 | |
sgrsn | 0:f1459eec7228 | 119 | i2c master(p28, p27); |
sgrsn | 0:f1459eec7228 | 120 | BusOut LEDs(LED1, LED2, LED3, LED4); |
sgrsn | 0:f1459eec7228 | 121 | Timer timer; |
sgrsn | 5:a5dc3090ba44 | 122 | JY901 jy901(&master, &timer); // gyro sensor |
sgrsn | 1:f102831401a8 | 123 | MakePath myPath; |
sgrsn | 0:f1459eec7228 | 124 | |
sgrsn | 5:a5dc3090ba44 | 125 | // this function is Robot Main Program |
sgrsn | 0:f1459eec7228 | 126 | void controlFromGcode() |
sgrsn | 0:f1459eec7228 | 127 | { |
sgrsn | 5:a5dc3090ba44 | 128 | // Threshold for reaching target value |
sgrsn | 1:f102831401a8 | 129 | float threshold_x = 5; //[mm] |
sgrsn | 1:f102831401a8 | 130 | float threshold_y = 5; //[mm] |
sgrsn | 2:83fa142c5bcc | 131 | float threshold_theta = 1 * DEG_TO_RAD; |
sgrsn | 0:f1459eec7228 | 132 | |
sgrsn | 0:f1459eec7228 | 133 | // 角度補正係数 |
sgrsn | 0:f1459eec7228 | 134 | float L = 233; //[mm] |
sgrsn | 0:f1459eec7228 | 135 | |
sgrsn | 0:f1459eec7228 | 136 | Timer timer2; |
sgrsn | 0:f1459eec7228 | 137 | PID pid_x(&timer2); |
sgrsn | 0:f1459eec7228 | 138 | PID pid_y(&timer2); |
sgrsn | 0:f1459eec7228 | 139 | PID pid_theta(&timer2); |
sgrsn | 1:f102831401a8 | 140 | pid_x.setParameter(200.0, 0.0, 0.0); |
sgrsn | 1:f102831401a8 | 141 | pid_y.setParameter(200.0, 0.0, 0.0); |
sgrsn | 1:f102831401a8 | 142 | pid_theta.setParameter(100.0, 0.0, 0.0); |
sgrsn | 0:f1459eec7228 | 143 | |
sgrsn | 5:a5dc3090ba44 | 144 | // read the txt file for get target position |
sgrsn | 0:f1459eec7228 | 145 | // Gコード読み取り |
sgrsn | 0:f1459eec7228 | 146 | LocalFileSystem local("local"); |
sgrsn | 0:f1459eec7228 | 147 | int array[SIZE] = {}; |
sgrsn | 0:f1459eec7228 | 148 | FILE *fp = fopen( "/local/guide1.txt", "r"); |
sgrsn | 0:f1459eec7228 | 149 | MakeSequencer code(fp); |
sgrsn | 4:25ab7216447f | 150 | |
sgrsn | 4:25ab7216447f | 151 | //printf("size:%d\r\n", code.getGcodeSize()); |
sgrsn | 3:4ac32aff309c | 152 | for(int i = 0; i < code.getGcodeSize(); i++) |
sgrsn | 3:4ac32aff309c | 153 | { |
sgrsn | 3:4ac32aff309c | 154 | code.getGcode(i,sizeof(array)/sizeof(int),array); |
sgrsn | 3:4ac32aff309c | 155 | printf("%d, %d, %d\r\n", array[0], array[1], i); |
sgrsn | 3:4ac32aff309c | 156 | } |
sgrsn | 0:f1459eec7228 | 157 | |
sgrsn | 5:a5dc3090ba44 | 158 | int row = 1; // row of txt file |
sgrsn | 5:a5dc3090ba44 | 159 | float v[4] = {}; // frequency each wheel |
sgrsn | 0:f1459eec7228 | 160 | |
sgrsn | 1:f102831401a8 | 161 | Timer temp_t; |
sgrsn | 1:f102831401a8 | 162 | CountWheel counter[4] = {CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t), CountWheel(&temp_t)}; |
sgrsn | 1:f102831401a8 | 163 | float wheel_rad[4] = {}; |
sgrsn | 1:f102831401a8 | 164 | |
sgrsn | 5:a5dc3090ba44 | 165 | //TextLCD lcd(p24, p26, p27, p28, p29, p30); |
sgrsn | 1:f102831401a8 | 166 | |
sgrsn | 1:f102831401a8 | 167 | float x_robot = 0; |
sgrsn | 1:f102831401a8 | 168 | float y_robot = 0; |
sgrsn | 1:f102831401a8 | 169 | float theta_robot = 0; |
sgrsn | 1:f102831401a8 | 170 | |
sgrsn | 1:f102831401a8 | 171 | |
sgrsn | 1:f102831401a8 | 172 | // 目標位置把握 |
sgrsn | 1:f102831401a8 | 173 | code.getGcode(row,sizeof(array)/sizeof(int),array); |
sgrsn | 1:f102831401a8 | 174 | float x_desire = array[0]; |
sgrsn | 1:f102831401a8 | 175 | float y_desire = array[1]; |
sgrsn | 1:f102831401a8 | 176 | float theta_desire = float(array[2]) *DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 177 | int pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot); |
sgrsn | 1:f102831401a8 | 178 | |
sgrsn | 1:f102831401a8 | 179 | int path = 0; |
sgrsn | 0:f1459eec7228 | 180 | |
sgrsn | 0:f1459eec7228 | 181 | while(1) |
sgrsn | 0:f1459eec7228 | 182 | { |
sgrsn | 4:25ab7216447f | 183 | nh.spinOnce(); |
sgrsn | 5:a5dc3090ba44 | 184 | // 自己位置推定(Estimate Self-localization) |
sgrsn | 1:f102831401a8 | 185 | theta_robot = jy901.calculateAngleOnlyGyro() * DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 186 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 1:f102831401a8 | 187 | { |
sgrsn | 1:f102831401a8 | 188 | wheel_rad[i] = counter[i].getRadian(v[i]); |
sgrsn | 1:f102831401a8 | 189 | } |
sgrsn | 2:83fa142c5bcc | 190 | float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3]) * wheel_r*0.3535534 * 0.67; |
sgrsn | 2:83fa142c5bcc | 191 | float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3]) * wheel_r*0.3535534 * 0.67; |
sgrsn | 0:f1459eec7228 | 192 | |
sgrsn | 1:f102831401a8 | 193 | x_robot += dx * cos(theta_robot) + dy * sin(-theta_robot); |
sgrsn | 1:f102831401a8 | 194 | y_robot += dy * cos(theta_robot) + dx * sin(theta_robot); |
sgrsn | 0:f1459eec7228 | 195 | |
sgrsn | 5:a5dc3090ba44 | 196 | // caliculate error of current position |
sgrsn | 0:f1459eec7228 | 197 | // 目標位置判定 |
sgrsn | 0:f1459eec7228 | 198 | float err_x = x_desire - x_robot; |
sgrsn | 0:f1459eec7228 | 199 | float err_y = y_desire - y_robot; |
sgrsn | 0:f1459eec7228 | 200 | float err_theta = theta_desire - theta_robot; |
sgrsn | 0:f1459eec7228 | 201 | |
sgrsn | 1:f102831401a8 | 202 | //printf("%f, %f, %d\r\n", theta_desire, theta_robot, row); |
sgrsn | 5:a5dc3090ba44 | 203 | //lcd.printf("%f, %f, %f, %f, %d\r\n",x_desire, y_desire, x_robot, y_robot, row); |
sgrsn | 1:f102831401a8 | 204 | //printf("%f, %f, %f, %d\r\n", err_x, err_y, err_theta, row); |
sgrsn | 1:f102831401a8 | 205 | |
sgrsn | 5:a5dc3090ba44 | 206 | // if Reach target position |
sgrsn | 0:f1459eec7228 | 207 | // 目標位置到達 |
sgrsn | 0:f1459eec7228 | 208 | if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) |
sgrsn | 0:f1459eec7228 | 209 | { |
sgrsn | 5:a5dc3090ba44 | 210 | // At first, Stop all Motor |
sgrsn | 0:f1459eec7228 | 211 | // 車輪を停止 |
sgrsn | 0:f1459eec7228 | 212 | coastAllMotor(); |
sgrsn | 1:f102831401a8 | 213 | //pid_x.reset(); |
sgrsn | 1:f102831401a8 | 214 | //pid_y.reset(); |
sgrsn | 1:f102831401a8 | 215 | //pid_theta.reset(); |
sgrsn | 1:f102831401a8 | 216 | wait_ms(500); |
sgrsn | 5:a5dc3090ba44 | 217 | |
sgrsn | 5:a5dc3090ba44 | 218 | // gyro reset |
sgrsn | 1:f102831401a8 | 219 | jy901.reset(); |
sgrsn | 0:f1459eec7228 | 220 | |
sgrsn | 5:a5dc3090ba44 | 221 | // update target position |
sgrsn | 0:f1459eec7228 | 222 | // Gコードを次の行に |
sgrsn | 0:f1459eec7228 | 223 | row++; |
sgrsn | 3:4ac32aff309c | 224 | if(row >= code.getGcodeSize()) |
sgrsn | 1:f102831401a8 | 225 | { |
sgrsn | 1:f102831401a8 | 226 | row = 0; |
sgrsn | 3:4ac32aff309c | 227 | coastAllMotor(); |
sgrsn | 4:25ab7216447f | 228 | startable = false; |
sgrsn | 4:25ab7216447f | 229 | while(!startable) |
sgrsn | 4:25ab7216447f | 230 | { |
sgrsn | 4:25ab7216447f | 231 | nh.spinOnce(); |
sgrsn | 4:25ab7216447f | 232 | wait_ms(1); |
sgrsn | 4:25ab7216447f | 233 | } |
sgrsn | 1:f102831401a8 | 234 | } |
sgrsn | 1:f102831401a8 | 235 | |
sgrsn | 3:4ac32aff309c | 236 | jy901.reset(); |
sgrsn | 3:4ac32aff309c | 237 | v[0] = 0; |
sgrsn | 3:4ac32aff309c | 238 | v[1] = 0; |
sgrsn | 3:4ac32aff309c | 239 | v[2] = 0; |
sgrsn | 3:4ac32aff309c | 240 | v[3] = 0; |
sgrsn | 3:4ac32aff309c | 241 | |
sgrsn | 5:a5dc3090ba44 | 242 | // get next target position |
sgrsn | 1:f102831401a8 | 243 | // 目標位置把握 |
sgrsn | 0:f1459eec7228 | 244 | code.getGcode(row, ArraySize(array), array); |
sgrsn | 1:f102831401a8 | 245 | x_desire = array[0]; |
sgrsn | 1:f102831401a8 | 246 | y_desire = array[1]; |
sgrsn | 1:f102831401a8 | 247 | theta_desire = float(array[2]) *DEG_TO_RAD; |
sgrsn | 1:f102831401a8 | 248 | pathSize = myPath.makePath(x_desire, y_desire, theta_desire*RAD_TO_DEG, x_robot, y_robot, theta_robot*RAD_TO_DEG); |
sgrsn | 1:f102831401a8 | 249 | path = 0; |
sgrsn | 0:f1459eec7228 | 250 | } |
sgrsn | 0:f1459eec7228 | 251 | |
sgrsn | 5:a5dc3090ba44 | 252 | // caliculate velosity |
sgrsn | 0:f1459eec7228 | 253 | // 目標速度計算 |
sgrsn | 0:f1459eec7228 | 254 | else |
sgrsn | 0:f1459eec7228 | 255 | { |
sgrsn | 0:f1459eec7228 | 256 | // 慣性座標での速度 |
sgrsn | 1:f102831401a8 | 257 | float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); |
sgrsn | 1:f102831401a8 | 258 | float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); |
sgrsn | 1:f102831401a8 | 259 | float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); |
sgrsn | 1:f102831401a8 | 260 | path++; |
sgrsn | 1:f102831401a8 | 261 | if(path >= pathSize) path = pathSize-1; |
sgrsn | 0:f1459eec7228 | 262 | |
sgrsn | 0:f1459eec7228 | 263 | // ロボット座標での速度 |
sgrsn | 0:f1459eec7228 | 264 | float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; |
sgrsn | 0:f1459eec7228 | 265 | float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; |
sgrsn | 0:f1459eec7228 | 266 | |
sgrsn | 0:f1459eec7228 | 267 | // 各車輪の速度 |
sgrsn | 0:f1459eec7228 | 268 | v[0] = -x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 269 | v[1] = x_dot - y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 270 | v[2] = x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 271 | v[3] = -x_dot + y_dot - L * theta_dot; |
sgrsn | 0:f1459eec7228 | 272 | |
sgrsn | 0:f1459eec7228 | 273 | // 本当はこうするべき |
sgrsn | 0:f1459eec7228 | 274 | // f = v * ppr / ( 2*PI * r); |
sgrsn | 0:f1459eec7228 | 275 | |
sgrsn | 5:a5dc3090ba44 | 276 | // Control Motor velocity |
sgrsn | 0:f1459eec7228 | 277 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:f1459eec7228 | 278 | { |
sgrsn | 1:f102831401a8 | 279 | controlMotor(i, (int)v[i]); |
sgrsn | 0:f1459eec7228 | 280 | } |
sgrsn | 0:f1459eec7228 | 281 | } |
sgrsn | 0:f1459eec7228 | 282 | } |
sgrsn | 0:f1459eec7228 | 283 | } |
sgrsn | 0:f1459eec7228 | 284 | |
sgrsn | 0:f1459eec7228 | 285 | int main() |
sgrsn | 0:f1459eec7228 | 286 | { |
sgrsn | 4:25ab7216447f | 287 | nh.getHardware()->setBaud(9600); |
sgrsn | 4:25ab7216447f | 288 | nh.initNode(); |
sgrsn | 4:25ab7216447f | 289 | nh.subscribe(sub); |
sgrsn | 0:f1459eec7228 | 290 | coastAllMotor(); |
sgrsn | 5:a5dc3090ba44 | 291 | // gyro sensor calibration |
sgrsn | 5:a5dc3090ba44 | 292 | // Please wait 5 seconds... |
sgrsn | 1:f102831401a8 | 293 | jy901.calibrateAll(5000); |
sgrsn | 5:a5dc3090ba44 | 294 | |
sgrsn | 5:a5dc3090ba44 | 295 | // Robot start when receive the ROS topic |
sgrsn | 4:25ab7216447f | 296 | while(!startable) |
sgrsn | 1:f102831401a8 | 297 | { |
sgrsn | 4:25ab7216447f | 298 | nh.spinOnce(); |
sgrsn | 4:25ab7216447f | 299 | wait_ms(1); |
sgrsn | 1:f102831401a8 | 300 | } |
sgrsn | 4:25ab7216447f | 301 | // main function |
sgrsn | 0:f1459eec7228 | 302 | controlFromGcode(); |
sgrsn | 0:f1459eec7228 | 303 | } |
sgrsn | 0:f1459eec7228 | 304 | |
sgrsn | 1:f102831401a8 | 305 | int controlMotor(int ch, int frequency) |
sgrsn | 0:f1459eec7228 | 306 | { |
sgrsn | 0:f1459eec7228 | 307 | int dir = COAST; |
sgrsn | 0:f1459eec7228 | 308 | int size = 4; |
sgrsn | 0:f1459eec7228 | 309 | if(ch < 0 || ch > 3) |
sgrsn | 0:f1459eec7228 | 310 | { |
sgrsn | 0:f1459eec7228 | 311 | //channel error |
sgrsn | 1:f102831401a8 | 312 | return 0; |
sgrsn | 0:f1459eec7228 | 313 | } |
sgrsn | 0:f1459eec7228 | 314 | else |
sgrsn | 0:f1459eec7228 | 315 | { |
sgrsn | 0:f1459eec7228 | 316 | if(frequency > 0) |
sgrsn | 0:f1459eec7228 | 317 | { |
sgrsn | 0:f1459eec7228 | 318 | dir = CW; |
sgrsn | 0:f1459eec7228 | 319 | } |
sgrsn | 0:f1459eec7228 | 320 | else if(frequency < 0) |
sgrsn | 0:f1459eec7228 | 321 | { |
sgrsn | 0:f1459eec7228 | 322 | dir = CCW; |
sgrsn | 0:f1459eec7228 | 323 | frequency = -frequency; |
sgrsn | 0:f1459eec7228 | 324 | } |
sgrsn | 0:f1459eec7228 | 325 | else |
sgrsn | 0:f1459eec7228 | 326 | { |
sgrsn | 0:f1459eec7228 | 327 | dir = BRAKE; |
sgrsn | 0:f1459eec7228 | 328 | } |
sgrsn | 0:f1459eec7228 | 329 | // 周波数制限 脱調を防ぐ |
sgrsn | 0:f1459eec7228 | 330 | if(frequency > MaxFrequency) frequency = MaxFrequency; |
sgrsn | 1:f102831401a8 | 331 | //else if(frequency < MinFrequency) frequency = MinFrequency; |
sgrsn | 0:f1459eec7228 | 332 | |
sgrsn | 0:f1459eec7228 | 333 | master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); |
sgrsn | 0:f1459eec7228 | 334 | master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); |
sgrsn | 1:f102831401a8 | 335 | |
sgrsn | 1:f102831401a8 | 336 | return frequency; |
sgrsn | 0:f1459eec7228 | 337 | } |
sgrsn | 0:f1459eec7228 | 338 | } |
sgrsn | 0:f1459eec7228 | 339 | |
sgrsn | 0:f1459eec7228 | 340 | |
sgrsn | 0:f1459eec7228 | 341 | void coastAllMotor() |
sgrsn | 0:f1459eec7228 | 342 | { |
sgrsn | 0:f1459eec7228 | 343 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:f1459eec7228 | 344 | { |
sgrsn | 0:f1459eec7228 | 345 | master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4); |
sgrsn | 0:f1459eec7228 | 346 | } |
sgrsn | 4:25ab7216447f | 347 | } |