120 lines
3.3 KiB
C
120 lines
3.3 KiB
C
/*
|
|
* main.c
|
|
*
|
|
* Created on: Aug 2, 2016
|
|
* Author: tkl
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
|
|
#include "driver.h"
|
|
#include "board.h"
|
|
#include "stack.h"
|
|
#include "queue.h"
|
|
#include "kernel.h"
|
|
#include "driver.h"
|
|
#include "list.h"
|
|
#include "shell.h"
|
|
|
|
struct engine_ctrl {
|
|
const struct driver *pwm;
|
|
const struct driver *enable;
|
|
};
|
|
|
|
struct drive_ctrl {
|
|
struct engine_ctrl *left_forward;
|
|
struct engine_ctrl *left_backward;
|
|
struct engine_ctrl *right_forward;
|
|
struct engine_ctrl *right_backward;
|
|
};
|
|
|
|
|
|
static struct engine_ctrl right_forward = {
|
|
.pwm = &pwm_1,
|
|
.enable = &gpio_c1,
|
|
};
|
|
|
|
static struct engine_ctrl right_backward = {
|
|
.pwm = &pwm_2,
|
|
.enable = &gpio_c0,
|
|
};
|
|
|
|
static struct engine_ctrl left_forward = {
|
|
.pwm = &pwm_3,
|
|
.enable = &gpio_c3,
|
|
};
|
|
|
|
static struct engine_ctrl left_backward = {
|
|
.pwm = &pwm_4,
|
|
.enable = &gpio_c2,
|
|
};
|
|
|
|
static struct drive_ctrl drive_ctrl = {
|
|
.left_forward = &left_forward,
|
|
.left_backward = &left_backward,
|
|
.right_forward = &right_forward,
|
|
.right_backward = &right_backward,
|
|
};
|
|
|
|
#define TH_STACK_SIZE 256
|
|
stack_t th_stack[TH_STACK_SIZE];
|
|
struct thread_context th_ctx;
|
|
static void th_func(void *arg)
|
|
{
|
|
unsigned int duty = 0;
|
|
|
|
/* open enable pins */
|
|
open(drive_ctrl.left_forward->enable);
|
|
write(drive_ctrl.left_forward->enable, "0", 1);
|
|
open(drive_ctrl.left_backward->enable);
|
|
write(drive_ctrl.left_backward->enable, "0", 1);
|
|
open(drive_ctrl.right_forward->enable);
|
|
write(drive_ctrl.right_forward->enable, "0", 1);
|
|
open(drive_ctrl.right_backward->enable);
|
|
write(drive_ctrl.right_backward->enable, "0", 1);
|
|
|
|
/* open pwm's*/
|
|
open(drive_ctrl.left_backward->pwm);
|
|
ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
open(drive_ctrl.left_forward->pwm);
|
|
ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
open(drive_ctrl.right_backward->pwm);
|
|
ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
open(drive_ctrl.right_forward->pwm);
|
|
ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
|
|
/* enable enable pins */
|
|
write(drive_ctrl.left_forward->enable, "1", 1);
|
|
write(drive_ctrl.left_backward->enable, "1", 1);
|
|
write(drive_ctrl.right_forward->enable, "1", 1);
|
|
write(drive_ctrl.right_backward->enable, "1", 1);
|
|
while(1) {
|
|
duty = 0;
|
|
ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
for(duty = 0; duty < 100; duty++) {
|
|
ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
sleep_ms(100);
|
|
}
|
|
duty = 0;
|
|
ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
sleep_ms(100);
|
|
for(duty = 0; duty < 100; duty++) {
|
|
ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
|
|
sleep_ms(100);
|
|
}
|
|
sleep_ms(100);
|
|
}
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
thread_create(&th_ctx, th_stack, TH_STACK_SIZE, th_func, NULL, THREAD_PRIO_LOW);
|
|
schedule_start();
|
|
|
|
return 0;
|
|
}
|