Revision 1307
Added turntable control code. Currently uses PI control and is having lots of mechanical issues
Also added a main_brad which moves the turntable back and forth a bit
added a return to home value for the turntable
| hardware_turntable.c | ||
|---|---|---|
| 1 | 1 |
#include "hardware_turntable.h" |
| 2 | 2 |
#include <encoders.h> |
| 3 | 3 |
|
| 4 |
#define TURNTABLE_ENCODER_ZERO 737 |
|
| 5 |
|
|
| 6 |
#define MOTOR_TURNTABLE_SET motor1_set |
|
| 7 |
#define TURNTABLE_ENCODER RIGHT |
|
| 8 |
#define TURNTABLE_MAX_PWM 255 |
|
| 9 |
|
|
| 10 |
#define TURNTABLE_TOLERANCE 5 |
|
| 11 |
#define TURNTABLE_STABLE_READINGS 100 |
|
| 12 |
|
|
| 13 |
//180 seems to be just below |
|
| 14 |
#define TURNTABLE_PWM_OFFSET 160 |
|
| 15 |
#define TURNTABLE_P 2 |
|
| 16 |
|
|
| 17 |
//this seems like it should be between 50:200 |
|
| 18 |
//100 is a little slow, but probably good |
|
| 19 |
#define TURNTABLE_I_DIV 100 |
|
| 20 |
|
|
| 21 |
int16_t turntable_encoder_offset = 0; |
|
| 22 |
|
|
| 4 | 23 |
void turntable_init () |
| 5 | 24 |
{
|
| 6 | 25 |
int e; |
| 7 | 26 |
encoders_init(); |
| 8 | 27 |
|
| 28 |
encoder_wait(1); |
|
| 29 |
|
|
| 9 | 30 |
// get a reading to check alignment |
| 10 |
e = encoder_read(LEFT);
|
|
| 31 |
e = encoder_read(TURNTABLE_ENCODER);
|
|
| 11 | 32 |
if(e==ENCODER_MAGNET_FAILURE) |
| 12 | 33 |
usb_puts("# ERROR: turntable encoder magnet failure!!" NL);
|
| 13 | 34 |
else if(e==ENCODER_MISALIGNED) |
| 14 | 35 |
usb_puts("# ERROR: turntable encoder misaligned!!" NL);
|
| 15 | 36 |
else if(e==ENCODER_DATA_NOT_READY) |
| 16 | 37 |
usb_puts("# ERROR: turntable encoder data not ready!!" NL);
|
| 17 |
else |
|
| 18 |
usb_puts("# Turntable encoder initialized" NL);
|
|
| 19 | 38 |
|
| 39 |
//get the offset |
|
| 40 |
turntable_encoder_offset = e - TURNTABLE_ENCODER_ZERO; |
|
| 41 |
//make it be less than 512 |
|
| 42 |
while(turntable_encoder_offset > 512) |
|
| 43 |
turntable_encoder_offset -= 1024; |
|
| 44 |
while(turntable_encoder_offset < -512) |
|
| 45 |
turntable_encoder_offset += 1024; |
|
| 46 |
|
|
| 47 |
|
|
| 20 | 48 |
motors_init(); |
| 21 | 49 |
} |
| 22 | 50 |
|
| 23 | 51 |
void turntable_rotate_to_position (int16_t position) |
| 24 | 52 |
{
|
| 25 |
uint16_t turntable; |
|
| 53 |
int16_t turntable=0; |
|
| 54 |
uint8_t direction=0, last_direction=0; |
|
| 55 |
int16_t error = 0; |
|
| 26 | 56 |
|
| 27 |
return; |
|
| 57 |
//must be 16 bit to check for overflow |
|
| 58 |
uint16_t power=0; |
|
| 59 |
uint8_t cycle_count=0; |
|
| 28 | 60 |
|
| 29 | 61 |
usb_puts ("# Turntable rotating to ");
|
| 30 | 62 |
usb_puti (position); |
| 31 | 63 |
usb_puts (NL); |
| 32 |
|
|
| 33 |
//TODO: p control |
|
| 34 |
//TODO: proper direction |
|
| 35 |
motor1_set(1, 180); |
|
| 36 | 64 |
|
| 37 |
while((turntable = turntable_get_position()) <= position) |
|
| 65 |
/* MOTOR_TURNTABLE_SET(1, TURNTABLE_PWM_OFFSET); |
|
| 66 |
|
|
| 67 |
delay_ms(1000); |
|
| 68 |
return; |
|
| 69 |
*/ |
|
| 70 |
|
|
| 71 |
while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE) |
|
| 38 | 72 |
{
|
| 39 |
delay_ms(1); |
|
| 40 |
usb_puti(turntable); |
|
| 41 |
usb_puts(NL); |
|
| 73 |
|
|
| 74 |
while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE) |
|
| 75 |
{
|
|
| 76 |
//figure out which direction to set the motor |
|
| 77 |
last_direction = direction; |
|
| 78 |
direction = turntable < position; |
|
| 42 | 79 |
|
| 43 |
if(turntable >= 1024) |
|
| 44 |
{
|
|
| 45 |
usb_puts("# ERROR: magnetic encoder error!" NL);
|
|
| 46 |
break; |
|
| 80 |
//clear the accumuliated error when switching directions |
|
| 81 |
if(last_direction != direction) |
|
| 82 |
error = 0; |
|
| 83 |
|
|
| 84 |
|
|
| 85 |
//only add to the error every TURNTABLE_I_DIV cycles |
|
| 86 |
if(++cycle_count > TURNTABLE_I_DIV) |
|
| 87 |
{
|
|
| 88 |
error += turntable - position; |
|
| 89 |
cycle_count = 0; |
|
| 90 |
} |
|
| 91 |
|
|
| 92 |
power = abs(TURNTABLE_P*(turntable - position) + error) + TURNTABLE_PWM_OFFSET; |
|
| 93 |
if(power > TURNTABLE_MAX_PWM) |
|
| 94 |
power = TURNTABLE_MAX_PWM; |
|
| 95 |
MOTOR_TURNTABLE_SET(direction, power); |
|
| 96 |
|
|
| 97 |
|
|
| 98 |
/* usb_puti(turntable); */ |
|
| 99 |
/* usb_puts(NL); */ |
|
| 100 |
|
|
| 101 |
if(turntable >= 1024) |
|
| 102 |
{
|
|
| 103 |
usb_puts("# ERROR: magnetic encoder error!" NL);
|
|
| 104 |
break; |
|
| 105 |
} |
|
| 106 |
|
|
| 107 |
usb_puti(error); |
|
| 108 |
usb_puts(" ");
|
|
| 109 |
usb_puti(direction); |
|
| 110 |
usb_puts(" ");
|
|
| 111 |
usb_puti(power); |
|
| 112 |
usb_puts(NL); |
|
| 113 |
|
|
| 114 |
delay_ms(1); |
|
| 115 |
|
|
| 116 |
encoder_wait(1); |
|
| 47 | 117 |
} |
| 118 |
|
|
| 119 |
encoder_wait(TURNTABLE_STABLE_READINGS); |
|
| 48 | 120 |
} |
| 49 | 121 |
|
| 50 |
motor1_set(1, 0); |
|
| 122 |
|
|
| 123 |
MOTOR_TURNTABLE_SET(direction, 0); |
|
| 124 |
|
|
| 125 |
//just to prevent resetting |
|
| 126 |
delay_ms(50); |
|
| 51 | 127 |
|
| 52 | 128 |
usb_puts ("# Turntable position reached" NL);
|
| 53 | 129 |
} |
| 54 | 130 |
|
| 55 | 131 |
uint16_t turntable_get_position (void) |
| 56 | 132 |
{
|
| 57 |
//TODO: convert units
|
|
| 58 |
return encoder_read(RIGHT);
|
|
| 133 |
return encoder_get_dx(TURNTABLE_ENCODER) + turntable_encoder_offset;
|
|
| 134 |
// return encoder_read(TURNTABLE_ENCODER);
|
|
| 59 | 135 |
} |
Also available in: Unified diff