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