ObjectMeasurementProject/motor_control.c at main · Radifical/ObjectMeasurementProject · GitHub

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
#include "motor_control.h"
#include "system_config.h"
#include <string.h>

// Initialize motor control system
void motor_init(motor_control_t *motor) {
    if (motor == NULL) return;

    motor->state = MOTOR_STOP;
    motor->position = MOTOR_HOME_POSITION;
    motor->step_count = 0;
    motor->is_moving = false;
}

// Set motor direction and speed
void motor_set_direction(motor_control_t *motor, motor_state_t direction) {
    if (motor == NULL) return;

    motor->state = direction;

    // Set hardware direction pin based on state
    // This would interface with FPGA GPIO
    // gpio_write(MOTOR_DIR_PIN, (direction == MOTOR_FORWARD) ? 1 : 0);
}

// Start motor movement
void motor_start(motor_control_t *motor) {
    if (motor == NULL) return;

    motor->is_moving = true;

    // Enable PWM output to motor
    motor_generate_pwm(50); // 50% duty cycle for forward movement
}

// Stop motor movement
void motor_stop(motor_control_t *motor) {
    if (motor == NULL) return;

    motor->is_moving = false;
    motor->state = MOTOR_STOP;

    // Disable PWM output
    motor_generate_pwm(0);
}

// Update motor state (call periodically)
void motor_update(motor_control_t *motor) {
    if (motor == NULL) return;

    if (!motor->is_moving) {
        return;
    }

    // Update position based on motor state
    switch (motor->state) {
        case MOTOR_FORWARD:
            if (motor->position < PLATFORM_LENGTH_STEPS) {
                motor->position += MOTOR_STEP_SIZE;
                motor->step_count++;
            } else {
                // Reached end of platform, reverse direction
                motor_set_direction(motor, MOTOR_BACKWARD);
            }
            break;

        case MOTOR_BACKWARD:
            if (motor->position > MOTOR_HOME_POSITION) {
                motor->position -= MOTOR_STEP_SIZE;
                motor->step_count++;
            } else {
                // Reached home position
                motor_stop(motor);
                motor->position = MOTOR_HOME_POSITION;
            }
            break;

        case MOTOR_RETURN_HOME:
            if (motor->position > MOTOR_HOME_POSITION) {
                motor->position -= MOTOR_STEP_SIZE;
            } else {
                motor_stop(motor);
                motor->position = MOTOR_HOME_POSITION;
            }
            break;

        case MOTOR_STOP:
        default:
            motor_stop(motor);
            break;
    }
}

// Get current motor position in steps
uint32_t motor_get_position(motor_control_t *motor) {
    if (motor == NULL) return 0;
    return motor->position;
}

// Move motor to home position
void motor_return_home(motor_control_t *motor) {
    if (motor == NULL) return;

    if (motor->position > MOTOR_HOME_POSITION) {
        motor_set_direction(motor, MOTOR_RETURN_HOME);
        motor_start(motor);
    } else {
        motor_stop(motor);
    }
}

// Generate PWM signal for motor speed control
// This is a simplified implementation - actual PWM would use hardware timers
void motor_generate_pwm(uint8_t duty_cycle) {
    // In a real implementation, this would configure FPGA timers/PWM modules
    // For now, this is a placeholder that would interface with hardware
    // Example: configure_timer_pwm(MOTOR_PWM_PIN, duty_cycle, MOTOR_PWM_FREQ_HZ);

    // Placeholder implementation
    (void)duty_cycle; // Suppress unused parameter warning
}