1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
| #include <math.h> #include <stdint.h>
void step_motor_x(int steps, int dir); void step_motor_y(int steps, int dir); int read_encoder_x(void); int read_encoder_y(void); void delay_us(uint32_t us);
#define STEPS_PER_MM 100 #define MAX_SPEED 1000 #define ACCELERATION 2000
#define KP 0.5 #define KI 0.01 #define KD 0.1 #define DT 0.001
typedef struct { float kp, ki, kd; float integral; float last_error; float max_output; } PIDController;
void pid_init(PIDController *pid) { pid->kp = KP; pid->ki = KI; pid->kd = KD; pid->integral = 0; pid->last_error = 0; pid->max_output = MAX_SPEED; }
float pid_compute(PIDController *pid, int target_pos, int actual_pos) { float error = target_pos - actual_pos; pid->integral += error * DT; float derivative = (error - pid->last_error) / DT; float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative; if (output > pid->max_output) output = pid->max_output; if (output < -pid->max_output) output = -pid->max_output; pid->last_error = error; return output; }
void pid_move(int target_steps, int dir, int axis) { PIDController pid; pid_init(&pid); int current_pos = (axis == 0) ? read_encoder_x() : read_encoder_y(); int step_count = 0; int total_steps = abs(target_steps);
while (step_count < total_steps) { current_pos = (axis == 0) ? read_encoder_x() : read_encoder_y(); float speed = pid_compute(&pid, step_count, current_pos); if (axis == 0) step_motor_x(1, dir); else step_motor_y(1, dir); delay_us(1000000 / (int)fabs(speed)); step_count++; } }
void draw_line(int x0, int y0, int x1, int y1) { int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1; int dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1; int err = (dx > dy ? dx : -dy) / 2, e2;
while (1) { pid_move(1, x0 < x1 ? 1 : 0, 0); pid_move(1, y0 < y1 ? 1 : 0, 1); if (x0 == x1 && y0 == y1) break; e2 = err; if (e2 > -dx) { err -= dy; x0 += sx; } if (e2 < dy) { err += dx; y0 += sy; } } }
void draw_circle(int xc, int yc, int r) { int x = 0, y = r; int d = 3 - 2 * r; while (x <= y) { pid_move(1, (xc + x) < xc ? 0 : 1, 0); pid_move(1, (yc + y) < yc ? 0 : 1, 1); pid_move(1, (xc - x) < xc ? 0 : 1, 0); pid_move(1, (yc + y) < yc ? 0 : 1, 1); pid_move(1, (xc + x) < xc ? 0 : 1, 0); pid_move(1, (yc - y) < yc ? 0 : 1, 1); pid_move(1, (xc - x) < xc ? 0 : 1, 0); pid_move(1, (yc - y) < yc ? 0 : 1, 1); pid_move(1, (xc + y) < xc ? 0 : 1, 0); pid_move(1, (yc + x) < yc ? 0 : 1, 1); pid_move(1, (xc - y) < xc ? 0 : 1, 0); pid_move(1, (yc + x) < yc ? 0 : 1, 1); pid_move(1, (xc + y) < xc ? 0 : 1, 0); pid_move(1, (yc - x) < yc ? 0 : 1, 1); pid_move(1, (xc - y) < xc ? 0 : 1, 0); pid_move(1, (yc - x) < yc ? 0 : 1, 1);
if (d < 0) { d += 4 * x + 6; } else { d += 4 * (x - y) + 10; y--; } x++; } }
void draw_sine_wave(int x_start, int x_end, int amplitude, int period) { for (int x = x_start; x <= x_end; x++) { int y = amplitude * sin(2 * M_PI * x / period); pid_move(1, x < (x + 1) ? 1 : 0, 0); pid_move(1, y < 0 ? 0 : 1, 1); } }
void draw_triangle(int x1, int y1, int x2, int y2, int x3, int y3) { draw_line(x1, y1, x2, y2); draw_line(x2, y2, x3, y3); draw_line(x3, y3, x1, y1); }
int main() { draw_triangle(0 * STEPS_PER_MM, 0 * STEPS_PER_MM, 50 * STEPS_PER_MM, 0 * STEPS_PER_MM, 25 * STEPS_PER_MM, 43 * STEPS_PER_MM);
draw_circle(50 * STEPS_PER_MM, 50 * STEPS_PER_MM, 20 * STEPS_PER_MM);
draw_sine_wave(0 * STEPS_PER_MM, 100 * STEPS_PER_MM, 10 * STEPS_PER_MM, 50 * STEPS_PER_MM);
return 0; }
|