Subversion Repositories Code-Repo

Rev

Rev 285 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 285 Rev 286
Line 114... Line 114...
114 
    Controller_Read(&ctrl);
 114 
    Controller_Read(&ctrl);
115 
 
 115 
 
116 
    uint16_t pwm_left = PWM_NOMINAL;
 116 
    uint16_t pwm_left = PWM_NOMINAL;
117 
    uint16_t pwm_right = PWM_NOMINAL;
 117 
    uint16_t pwm_right = PWM_NOMINAL;
118 
 
 118 
 
- 
 
 119 
    uint8_t median;
119 
#ifdef CONTROL_FROM_UART
 120 
    int16_t L_X, L_Y, R_X, R_Y;
- 
 
 121
120 
    uint8_t buffer[32];
 122 
    uint8_t buffer[32];
121 
    uint8_t length;
 123 
    uint8_t length;
122 
    uint8_t uart_state = UART_STATE_READ_CMD;
 124 
    uint8_t uart_state = UART_STATE_READ_CMD;
123 
    uint8_t uart_cmd;
 125 
    uint8_t uart_cmd;
124 
    while(1) {
 - 
 
125 
        length = UART_Read(buffer);
 - 
 
126 
        for (uint8_t i = 0; i < length; i++) {
 - 
 
127 
            if (uart_state == UART_STATE_READ_CMD) {
 - 
 
128 
                if (buffer[i] == UART_CMD_RESET) {
 - 
 
129 
                    RESET();
 - 
 
130 
                } else {
 - 
 
131 
                    // Read and save first byte (command)
 - 
 
132 
                    uart_cmd = buffer[i];
 - 
 
133 
                    uart_state = UART_STATE_READ_DATA;
 - 
 
134 
                }
 - 
 
135 
            } else if (uart_state == UART_STATE_READ_DATA) {
 - 
 
136 
                uart_state = UART_STATE_READ_CMD;
 - 
 
137 
 
 - 
 
138 
                // Process received data
 - 
 
139 
                if (uart_cmd == UART_CMD_LEFT_FORWARD) {
 - 
 
140 
                    pwm_left = PWM_NOMINAL + (uint16_t)(buffer[i] * 2);
 - 
 
141 
                    if (buffer[i] != 0) LED_2_On();
 - 
 
142 
                    else LED_2_Off();
 - 
 
143 
                } else if (uart_cmd == UART_CMD_LEFT_BACKWARD) {
 - 
 
144 
                    pwm_left = PWM_NOMINAL - (uint16_t)(buffer[i] * 2);
 - 
 
145 
                    if (buffer[i] != 0) LED_2_On();
 - 
 
146 
                    else LED_2_Off();
 - 
 
147 
                } else if (uart_cmd == UART_CMD_RIGHT_FORWARD) {
 - 
 
148 
                    pwm_right = PWM_NOMINAL + (uint16_t)(buffer[i] * 2);
 - 
 
149 
                    if (buffer[i] != 0) LED_1_On();
 - 
 
150 
                    else LED_1_Off();
 - 
 
151 
                } else if (uart_cmd == UART_CMD_RIGHT_BACKWARD) {
 - 
 
152 
                    pwm_right = PWM_NOMINAL - (uint16_t)(buffer[i] * 2);
 - 
 
153 
                    if (buffer[i] != 0) LED_1_On();
 - 
 
154 
                    else LED_1_Off();
 - 
 
155 
                }
 - 
 
156 
 
 - 
 
157 
                if (pwm_left > PWM_MAX)
 - 
 
158 
                    pwm_left = PWM_MAX;
 - 
 
159 
                if (pwm_left < PWM_MIN)
 - 
 
160 
                    pwm_left = PWM_MIN;
 - 
 
161 
 
 - 
 
162 
                if (pwm_right > PWM_MAX)
 - 
 
163 
                    pwm_right = PWM_MAX;
 - 
 
164 
                if (pwm_right < PWM_MIN)
 - 
 
165 
                    pwm_right = PWM_MIN;
 - 
 
166 
 
 126 
 
167 
                PWM_1_Set(pwm_right);
 - 
 
168 
                PWM_2_Set(pwm_left);
 - 
 
169 
            }
 - 
 
170 
        }
 - 
 
171 
    }
 - 
 
172 
#endif
 - 
 
173 
 
 - 
 
174 
#ifdef CONTROL_FROM_CONTROLLER
 - 
 
175 
    uint8_t median;
 - 
 
176 
    int16_t L_X, L_Y, R_X, R_Y;
 - 
 
177 
    while (1) {
 127 
    while (1) {
178 
        if (!Controller_Read(&ctrl)) {
 128 
        if (!Controller_Read(&ctrl)) {
179 
            median = ctrl.REF / 2;
 129 
            median = ctrl.REF / 2;
180 
            L_X = (int16_t)median - (int16_t)ctrl.L_X_CH;
 130 
            L_X = (int16_t)median - (int16_t)ctrl.L_X_CH;
181 
            L_Y = (int16_t)median - (int16_t)ctrl.L_Y_CH;
 131 
            L_Y = (int16_t)median - (int16_t)ctrl.L_Y_CH;
Line 228... Line 178...
228 
            PWM_1_Set(PWM_NOMINAL);
 178 
            PWM_1_Set(PWM_NOMINAL);
229 
            PWM_2_Set(PWM_NOMINAL);
 179 
            PWM_2_Set(PWM_NOMINAL);
230 
 
 180 
 
231 
            LED_1_Off();
 181 
            LED_1_Off();
232 
            LED_2_Off();
 182 
            LED_2_Off();
- 
 
 183 
 
- 
 
 184 
            // If no controller is attached, switch control to UART
- 
 
 185 
            while(1) {
- 
 
 186 
                length = UART_Read(buffer);
- 
 
 187 
                for (uint8_t i = 0; i < length; i++) {
- 
 
 188 
                    if (uart_state == UART_STATE_READ_CMD) {
- 
 
 189 
                        if (buffer[i] == UART_CMD_RESET) {
- 
 
 190 
                            RESET();
- 
 
 191 
                        } else {
- 
 
 192 
                            // Read and save first byte (command)
- 
 
 193 
                            uart_cmd = buffer[i];
- 
 
 194 
                            uart_state = UART_STATE_READ_DATA;
- 
 
 195 
                        }
- 
 
 196 
                    } else if (uart_state == UART_STATE_READ_DATA) {
- 
 
 197 
                        uart_state = UART_STATE_READ_CMD;
- 
 
 198 
 
- 
 
 199 
                        // Process received data
- 
 
 200 
                        if (uart_cmd == UART_CMD_LEFT_FORWARD) {
- 
 
 201 
                            pwm_left = PWM_NOMINAL + (uint16_t) (buffer[i] * 2);
- 
 
 202 
                            if (buffer[i] != 0) LED_2_On();
- 
 
 203 
                            else LED_2_Off();
- 
 
 204 
                        } else if (uart_cmd == UART_CMD_LEFT_BACKWARD) {
- 
 
 205 
                            pwm_left = PWM_NOMINAL - (uint16_t) (buffer[i] * 2);
- 
 
 206 
                            if (buffer[i] != 0) LED_2_On();
- 
 
 207 
                            else LED_2_Off();
- 
 
 208 
                        } else if (uart_cmd == UART_CMD_RIGHT_FORWARD) {
- 
 
 209 
                            pwm_right = PWM_NOMINAL + (uint16_t) (buffer[i] * 2);
- 
 
 210 
                            if (buffer[i] != 0) LED_1_On();
- 
 
 211 
                            else LED_1_Off();
- 
 
 212 
                        } else if (uart_cmd == UART_CMD_RIGHT_BACKWARD) {
- 
 
 213 
                            pwm_right = PWM_NOMINAL - (uint16_t) (buffer[i] * 2);
- 
 
 214 
                            if (buffer[i] != 0) LED_1_On();
- 
 
 215 
                            else LED_1_Off();
- 
 
 216 
                        }
- 
 
 217 
 
- 
 
 218 
                        if (pwm_left > PWM_MAX)
- 
 
 219 
                            pwm_left = PWM_MAX;
- 
 
 220 
                        if (pwm_left < PWM_MIN)
- 
 
 221 
                            pwm_left = PWM_MIN;
- 
 
 222 
 
- 
 
 223 
                        if (pwm_right > PWM_MAX)
- 
 
 224 
                            pwm_right = PWM_MAX;
- 
 
 225 
                        if (pwm_right < PWM_MIN)
- 
 
 226 
                            pwm_right = PWM_MIN;
- 
 
 227 
 
- 
 
 228 
                        PWM_1_Set(pwm_right);
- 
 
 229 
                        PWM_2_Set(pwm_left);
- 
 
 230 
                    }
- 
 
 231 
                }
- 
 
 232 
            }
233 
        }
 233 
        }
234 
    }
 234 
    }
235 
#endif
 - 
 
236 
}
 235 
}