/* IOCTL interface */
static int motor_ioctl (struct inode *in, struct file *fl, unsigned int cmd, \
unsigned long arg) {
int retval = 0, id;
unsigned long to_end;
struct cdev* p = in->i_cdev;
id = cdev_to_id (p);
switch (cmd) {
case MOTOR_ENABLE:
if ((int)arg)
gpio_set_value (g_enable[id], 0 ^ polarity[id]);
else
gpio_set_value (g_enable[id], 1 ^ polarity[id]);
break;
case MOTOR_DIR:
if ((int)arg)
gpio_set_value (g_dir[id], 1);
else
gpio_set_value (g_dir[id], 0);
break;
case MOTOR_PWM_ON:
pwm_start (pwmc[id]);
break;
case MOTOR_PWM_OFF:
pwm_stop (pwmc[id]);
break;
case MOTOR_PWM_SET:
//set the pwm period in ms
motor_pwm_set (pwmc[id], arg);
break;
case MOTOR_RESET:
steps[id] = 0; /* set the actual position as home */
break;
case MOTOR_STEPS:
steps_max[id] = arg; /* set the steps limit */
break;
case MOTOR_START:
if (g_step[id] == 0) {
} else {
if ((int)arg)
retval = pwm_set_handler (pwmc[id], &irq_steps_handler, NULL);
else
retval = pwm_set_handler (pwmc[id], NULL, NULL);
}
break;
case MOTOR_LOWPWR:
if ((int)arg)
gpio_set_value (g_lpwr[id], 1 ^ polarity[id]);
else
gpio_set_value (g_lpwr[id], 0 ^ polarity[id]);
break;
/* return steps_max-step */
case MOTOR_TO_END:
to_end = steps_max[id] - steps[id];
copy_to_user(&arg, &to_end, sizeof(unsigned long));
break;
default:
retval = -EINVAL;
}
return retval;
}
static int motor_add_one(unsigned int id, unsigned int *params)
{
int status, err;
struct cdev *motor_cdev;
struct platform_device *pdev;
struct gpio_pwm_platform_data pdata;
if ( mot_nump[id] < 4 ) {
printk(KERN_INFO "stepper: nothing to register for id: %d.\n", id);
return 0;
}
g_enable[id] = params[1];
g_dir[id] = params[2];
g_step[id] = params[3];
g_lpwr[id] = params[4];
polarity[id] = params[5];
/* sanity check */
if ( !( g_enable[id] && g_dir[id] && g_step[id])) {
printk(KERN_INFO "stepper: missing parameters, exit driver.\n");
goto err_para;
}
/* request and set pwm channel and gpio pins */
pdev = platform_device_alloc("gpio_pwm", g_step[id]);
if (!pdev) {
err = -ENOMEM;
goto err_para;
}
pdata.gpio = g_step[id];
err = platform_device_add_data(pdev, &pdata, sizeof(pdata));
if (err)
goto err;
err = platform_device_add(pdev);
if (err)
goto err;
pwmc[id] = pwm_request("gpio_pwm", g_step[id], "stepper");
if (pwmc[id] != NULL) {
goto err_pwm;
}
motor_pwm_set (pwmc[id], 0); /* set default pwm pulse */
if ( gpio_request(g_enable[id], "motor-enable") < 0 ) {
goto err_gpioenable;
}
gpio_direction_output(g_enable[id] ,0);
if ( gpio_request(g_dir[id], "motor-ccw") < 0) {
goto err_gpiodir;
}
gpio_direction_output(g_dir[id] ,0);
if (g_lpwr[id] != 0) {
if ( gpio_request(g_lpwr[id], "motor-lowpwr") < 0 ) {
goto err_gpiolwr;
}
gpio_direction_output(g_lpwr[id] ,0);
}
/* set to home */
steps[id] = 0;
/* alloc a new device number (major: dynamic, minor: 0) */
status = alloc_chrdev_region(&motor_devno, 0, 1, "motor");
/* create a new char device */
motor_cdev = cdev_alloc();
if(motor_cdev == NULL) {
status=-ENOMEM;
goto err_dev;
}
/*save the cdev for id's */
mot_map[id] = (int) motor_cdev;
motor_cdev->owner = THIS_MODULE;
motor_cdev->ops = &motor_fops;
status = cdev_add(motor_cdev, motor_devno, 1);
if(status){
goto err_dev;
}
device_create(motor_class, NULL, motor_devno, NULL, "motor%d", params[0]);
printk(KERN_INFO "stepper: motor%d registred on major: %u; minor: %u\n", \
params[0], MAJOR(motor_devno), MINOR(motor_devno));
return 0;
err:
printk(KERN_INFO "stepper: err\n");
err_dev:
printk(KERN_INFO "stepper: err_dev\n");
err_gpiolwr:
printk(KERN_INFO "stepper: err_gpiolwr\n");
err_gpiodir:
printk(KERN_INFO "stepper: err_gpiodir\n");
err_gpioenable:
printk(KERN_INFO "stepper: err_gpioenable\n");
err_gpiostep:
printk(KERN_INFO "stepper: err_gpiostep ");
err_pwm:
printk(KERN_INFO "stepper: err_pwm\n");
err_para:
printk(KERN_INFO "stepper: Error management not yet implemented. \
Please reboot your board %d\n",g_step[id]);
return -1;
}
// Current control loop ISR - 5 kHz
void __ISR(_TIMER_2_VECTOR, IPL5SOFT) Controller() {
char buffer[200];
// state querying code
switch(util_state_get()) {
case IDLE:
//set PWM to 0
motor_pwm_set(0);
//reset Eint to 0
Eint = 0;
break;
case PWM:
// set PWM to user specified value
break;
case TUNE:
if(StoringData) {
int E = 0, PWM = 0, temp1;
control_data d;
d.reference = 200*flag;
d.sensor = isense_amps();
E = d.reference - d.sensor;
Eint = Eint + E;
// integral anti-windup
if(Eint > EINTMAX) {Eint = EINTMAX;}
if(Eint < -EINTMAX) {Eint = -EINTMAX;}
//
d.control = Kp*((float) E) + Ki*((float) Eint);
PWM = ((int) d.control);
if(d.control < -100) {PWM = -100;}
if(d.control > 100) {PWM = 100;}
temp1 = motor_pwm_set(PWM);
if(StoringDataControl==1) {
util_buffer_write(d);
}
counter++;
if(counter==19)
{
counter = 0;
if(flag == 0) {temp = 1;}
if(flag == 1) { temp = 0;}
flag = temp;
}
}
else {
counter = 0;
Eint = 0;
}
break;
case TRACK:
if(StoringData) {
int E = 0, PWM = 0, temp1;
control_data d;
d.reference = reference_milliamps;
d.sensor = isense_amps();
E = d.reference - d.sensor;
Eint = Eint + E;
// integral anti-windup
if(Eint > EINTMAX) {Eint = EINTMAX;}
if(Eint < -EINTMAX) {Eint = -EINTMAX;}
//
d.control = Kp*((float) E) + Ki*((float) Eint);
PWM = ((int) d.control);
if(d.control < -100) {PWM = -100;}
if(d.control > 100) {PWM = 100;}
temp1 = motor_pwm_set(PWM);
if(StoringDataControl==1) {
util_buffer_write(d);
}
}
else {
Eint = 0;
reference_milliamps = 0;
}
break;
default:
//error, unknown state
NU32_LED2 = 0;
}
IFS0bits.T2IF = 0; // clear interrupt flag
}
