void Acquired_Taste(){//dump
motor_off();
shake();
ao();
enable_servos();
servo_set(0, 450,4);//half way up
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//back up
msleep(2000);
servo_set (0, 20, 2);//full up
ao();
motor(MOT_LEFT, 40);
motor(MOT_RIGHT, 30);//unjam if jammed
msleep(1000);
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//redump
msleep(1000);
ao();
disable_servos();
msleep(2000);
enable_servos();
motor(MOT_LEFT, 60);
motor(MOT_RIGHT, 30);
servo_set (0, 700, 2);//forward and drop
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);
servo_set (0, 800, 2);
disable_servos();
}
void claw_up(){//close claw, then raise claw and open it
//void servo_set(int port,int end,float time)
servo_set(SERV_CLAW,1500,.22);
//servo_set(SERV_WRIST,700,.4);
servo_set(SERV_WRIST,550,.5);
set_servo_position(SERV_CLAW,1200);
servo_set(SERV_WRIST,350,.2);
msleep(300);
set_servo_position(SERV_WRIST,550);
}
void score_gold(){
//set_servo_position(CLAWL,CLAW_DOWN);//fast
servo_set(CLAWL,CLAW_DOWN,1);
msleep(1000);
//set_servo_position(CLAWOC,CLAW_OPEN);//fast
servo_set(CLAWOC,CLAW_OPEN,1);//slow
msleep(1000);
set_servo_position(CLAWL,CLAW_UP);
msleep(500);
set_servo_position(CLAWOC,CLAW_CLOSE);
msleep(500);
}
void get_gold(){
squareup(-100,0.6);
set_servo_position(CLAWOC,CLAW_OPEN);
msleep(1000);
set_servo_position(CLAWL,CLAW_DOWN);
msleep(2000);
//set_servo_position(CLAWOC,CLAW_CLOSE);//fast
servo_set(CLAWOC,CLAW_CLOSE,2);//slow
msleep(1000);
//set_servo_position(CLAWL,CLAW_UP-50);//fast
servo_set(CLAWL,CLAW_UP-50,1);//slow
msleep(500);
}
void drive_dump()//dumps the basket into the caldera by driving
{
move_block_arm(BLA_MID);//get the arm out of the way
servo_set(BASKET_ARM, BA_UP, 1);//raise the arm
move_block_arm(BLA_UP);//driving position
time_drive(-50, -50, 1000);//drive back into the caldera
msleep(1000);//let stuff fall out
time_drive(50, 50, 500);//one more attempted dump
time_drive(-50, -50, 1000);//
time_drive(50, 50, 500);//get off the caldera a bit (hopefully reducing catching)
msleep(1000);//more time to fall out
move_block_arm(BLA_MID);//get the arm out of the way
servo_set(BASKET_ARM, BA_DOWN, 1);//bring the basket down
move_block_arm(BLA_UP);//driving position
}
void brain_run(void)
{
/* initialize components */
puts("[brain] initializing front distance sensor...");
if (srf02_init(&dist_front, CONF_DIST_FRONT_I2C, CONF_DIST_FRONT_ADDR) < 0) {
puts("[failed]");
return;
}
puts("[brain] initializing back distance sensor...");
if (srf02_init(&dist_back, CONF_DIST_BACK_I2C, CONF_DIST_BACK_ADDR) < 0) {
puts("[failed]");
return;
}
puts("[brain] initializing steering servo...");
if (servo_init(&steering, CONF_STEERING_PWM, CONF_STEERING_PWM_CHAN,
CONF_STEERING_MIN, CONF_STEERING_MAX) < 0) {
puts("[failed]");
return;
}
servo_set(&steering, CONF_STEERING_CENTER);
puts("[brain] initializing motor driver...");
if (motor_init(&mot, &mot_params)) {
puts("[failed]");
return;
}
/* go and have fun */
puts("[brain] components ready, all up an running!");
thread_create(stack, sizeof(stack),
CONF_PRIO_COL_DETECT, THREAD_CREATE_STACKTEST,
colllision_detection, NULL, "col detect");
}
int main(void)
{
int res;
int pos = (STEP_LOWER_BOUND + STEP_UPPER_BOUND) / 2;
int step = STEP;
puts("\nRIOT RC servo test");
puts("Connect an RC servo or scope to PWM_0 channel 0 to see anything");
res = servo_init(&servo, DEV, CHANNEL, SERVO_MIN, SERVO_MAX);
if (res < 0) {
puts("Errors while initializing servo");
return -1;
}
puts("Servo initialized.");
while (1) {
servo_set(&servo, pos);
pos += step;
if (pos <= STEP_LOWER_BOUND || pos >= STEP_UPPER_BOUND) {
step = -step;
}
vtimer_usleep(WAIT);
}
return 0;
}
// Receive data from the brain
void brain_rx_thread(void) {
uint8_t input;
while(1) {
// wait for input
while(!rx_ready(BRAIN)) yeild();
input = rx_byte(BRAIN);
switch(input) {
case 'M':
// receive motor command
brain_pack.reset();
do {
while(!rx_ready(BRAIN)) yeild();
input = rx_byte(BRAIN);
brain_pack.input((char)input);
} while( input != '\r');
if( !bt_control ) {
target_speed = brain_pack.reads8();
steer = brain_pack.readu8();
input = steer + STEER_OFFSET; // steering zero set
servo_set(0, input);
}
break;
case 'G':
passthrough(BRAIN, BT, 'G');
break;
default:
passthrough(BRAIN, BT, input);
break;
}
}
}
int servo_disable(int pin)
{
// 0 to time param causes servo funciton running
// in the other cog to disable the servo
int result = servo_set(pin, 0);
return result;
}
int16_t main(void) {
// printf("Starting Rocket Controller...\r\n");
init_clock();
init_ui();
init_pin();
init_timer();
init_i2c();
setup();
init_servo_driver(&sd1, &i2c3, 16000., 0x0);
init_servo(&servo4, &sd1, 0);
InitUSB();
U1IE = 0xFF; //setting up ISR for USB requests
U1EIE = 0xFF;
IFS5bits.USB1IF = 0; //flag
IEC5bits.USB1IE = 1; //enable
// uint32_t pid_command;
servo_set(&servo4, 1500, 0);
while (1) {
if (timer_flag(&timer1)) {
// Blink green light to show normal operation.
timer_lower(&timer1);
led_toggle(&led2);
// servo_set(&servo4, 1500, 0);
}
}
}
void _Pragma("entrypoint") test_ppm_task(void)
{
if( ppm_valid )
{
ppm_valid = FALSE;
ppm_cpt++;
radio_ok = TRUE;
radio_really_lost = FALSE;
time_since_last_ppm = 0;
last_radio_from_ppm();
if (last_radio_contains_avg_channels)
{
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
}
if (mode == MODE_MANUAL)
{
servo_set(last_radio);
}
}
else if (mode == MODE_MANUAL && radio_really_lost)
{
mode = MODE_AUTO;
}
if (time_since_last_ppm >= STALLED_TIME)
{
radio_ok = FALSE;
}
if (time_since_last_ppm >= REALLY_STALLED_TIME)
{
radio_really_lost = TRUE;
}
}
void brain_steer(int16_t dir)
{
dir = (dir / 2) + CONF_STEERING_CENTER;
if (dir < 0) {
dir = CONF_STEERING_MIN;
}
servo_set(&steering, (unsigned int)dir);
}
void check_failsafe_task(void)
{
if ((mode == MODE_MANUAL && !radio_ok) ||
(mode == MODE_AUTO && !mega128_ok))
{
servo_set(failsafe);
}
}
void _Pragma("entrypoint") check_failsafe_task(void)
{
if ((mode == MODE_MANUAL && !radio_ok) ||
(mode == MODE_AUTO && !mega128_ok))
{
servo_set(failsafe);
}
}
void pwm_run()
{
servo_start();
waitcnt(CNT + CLKFREQ/2);
while(1)
{
for (int i=0;i<4;i++)
servo_set(motors[i]->pin, motors[i]->current_val);
}
}
void check_failsafe_task(void)
{
//vPrintString("S_9 check_failsafe_task start! \n\r"); //SunnyBeike
//modified by wanbo
//if ((mode == MODE_MANUAL && !radio_ok) || (mode == MODE_AUTO && !mega128_ok))
{
servo_set(failsafe);
}
//vPrintString("S_9 check_failsafe_task end! \n\r"); //SunnyBeike
}
/**
* Parser fuction for the default state.
*
* This is the parser for the normal operation.
*
* \param m The received message
*/
void parse_default(tExtendedCAN *m) {
switch (m->data[0]) {
case CMD_PING:
last_heartbeat = timer_get_ms();
can_put_cmd(CMD_PONG, NULL, 0);
break;
case CMD_GET_VERSION:
{
char *version = "RK" XSTRING(MAJOR) "." XSTRING(MINOR);
uint8_t len = strlen(version);
if (len > 7) len = 7;
can_put_cmd(CMD_VERSION, (uint8_t*) version, len);
}
break;
case CMD_DRIBBLE:
{
ballhandler_set((int8_t)m->data[1],(int8_t)m->data[2]);
}
break;
case CMD_STOP:
{
stop_set((uint8_t)m->data[1]);
}
break;
case CMD_SERVO:
{
servo_set((uint8_t)m->data[1]);
}
break;
case CMD_LIGHT:
{
if( (uint8_t)m->data[1] > 0 )
{
RESET(OF_LED);
light = true;
}
else
{
SET(OF_LED);
light = false;
}
//debug("receive light");
}
break;
case 'm':
debug("manual");
manual_mode = true;
parse_data = parse_manual;
break;
default:
error("Command not implemented");
break;
}
}
/**
* Calibrates the ESC to understand our control range.
* Only needs to be run once. But I don't know how volatile
* the ESCs are so we should run this on startup.
*
* The protocol for this was found in the documentation bundled
* with the ESCs
**/
void PropServo::calibrate(){
print("Calibrating ESCs...");
servo_set(prop1pin, maxrpm);
servo_set(prop2pin, maxrpm);
servo_set(prop3pin, maxrpm);
servo_set(prop4pin, maxrpm);
pause(3000);
servo_set(prop1pin, stoprpm);
servo_set(prop2pin, stoprpm);
servo_set(prop3pin, stoprpm);
servo_set(prop4pin, stoprpm);
pause(3000);
print("done\n");
//other classes are free to manage the props now
PropServoStatus = 1;
}
void servo_controller_periodic_handler(void)
{
int i;
period_count++;
for (i = 0; i < SERVOn; i++)
if (period_count >= servo_motor[i].position) servo_reset(i);
if (period_count >= period) {
for (i = 0; i < SERVOn; i++) servo_set(i);
period_count = 0;
}
}
/*---------------------------------------------------------------------------*/
static int
configure(int type, int c)
{
switch(type) {
case SENSORS_ACTIVE:
if(c) {
if(!status(SENSORS_ACTIVE)) {
servo_init();
enabled = 1;
}
} else {
servo_off();
enabled = 1;
}
break;
case SERVO_SENSOR_A:
servo_set(0,c);
break;
case SERVO_SENSOR_B:
servo_set(1,c);
break;
}
return 0;
}
void check_mega128_values_task(void)
{
if ( !SpiIsSelected() && spi_was_interrupted )
{
if (mega128_receive_valid)
{
time_since_last_mega128 = 0;
mega128_ok = TRUE;
if (mode == MODE_AUTO)
servo_set(from_mega128.channels);
}
}
if (time_since_last_mega128 == STALLED_TIME) {
mega128_ok = FALSE;
}
}
int main( void )
{
int cycle_count = 0;
uint8_t servo_pos = 0;
avr_init();
adc_init( 0x70 );
puts( "$Id: sweep.c,v 2.0 2002/09/22 02:10:18 tramm Exp $" );
putnl();
while( 1 )
{
accel_task( adc_task );
if( ++cycle_count % 32 )
continue;
puthex( high_bits );
puthex( time() );
putc( ':' );
puts( " Ax=" ); puthex( a_x );
puts( " Ay=" ); puthex( a_y );
puts( " pos=" ); puthexs( servo_pos );
servo_set( 0, servo_pos );
servo_set( 1, servo_pos );
servo_set( 2, servo_pos );
servo_set( 3, servo_pos );
servo_set( 4, servo_pos );
servo_set( 5, servo_pos );
servo_set( 6, servo_pos );
servo_set( 7, servo_pos );
servo_pos += 8;
putnl();
}
return 0;
}
void _Pragma("entrypoint") check_mega128_values_task(void)
{
#ifndef WCET_ANALYSIS
if ( !SpiIsSelected() && spi_was_interrupted )
{
if (mega128_receive_valid)
{
time_since_last_mega128 = 0;
mega128_ok = TRUE;
if (mode == MODE_AUTO)
servo_set(from_mega128.channels);
}
}
if (time_since_last_mega128 == STALLED_TIME) {
mega128_ok = FALSE;
}
#endif
}
void check_mega128_values_task(void)
{
//vPrintString("S_8 check_mega128_values_task start! \n\r"); //SunnyBeike
// modified by wanbo
//if ( !SpiIsSelected() && spi_was_interrupted )
{
if (mega128_receive_valid)
{
time_since_last_mega128 = 0;
mega128_ok = TRUE;
if (mode == MODE_AUTO)
servo_set(from_mega128.channels);
}
}
//if (time_since_last_mega128 == STALLED_TIME)
{
mega128_ok = FALSE;
}
//vPrintString("S_8 check_mega128_values_task end! \n\r"); //SunnyBeike
}
/*
#ifndef PAPABENCH_SINGLE
int main( void )
{
fbw_init();
while( 1 )
{
fbw_schedule();
if(timer_periodic())
{
_1Hz++;
_20Hz++;
if (_1Hz >= 60)
{
_1Hz = 0;
}
if (_20Hz >= 3)
{
_20Hz = 0;
}
}
}
return 0;
}
#endif
*/
void test_ppm_task(void)
{
//vPrintString("S_1 test_ppm_task start! \n\r"); //SunnyBeike
// modified by wanbo
//if( ppm_valid )
{
ppm_valid = FALSE;
ppm_cpt++;
radio_ok = TRUE;
radio_really_lost = FALSE;
time_since_last_ppm = 0;
last_radio_from_ppm();
//if (last_radio_contains_avg_channels)
{
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
}
if (mode == MODE_MANUAL)
{
servo_set(last_radio);
}
}
//else if (mode == MODE_MANUAL && radio_really_lost)
{
mode = MODE_AUTO;
}
//if (time_since_last_ppm >= STALLED_TIME)
{
radio_ok = FALSE;
}
//if (time_since_last_ppm >= REALLY_STALLED_TIME)
{
radio_really_lost = TRUE;
}
//vPrintString("S_1 test_ppm_task end! \n\r"); //SunnyBeike
}
void cmd_servo(char *args)
{
char arg1[ARG_LENGTH];
char arg2[ARG_LENGTH];
int length = get_word(arg1, args, ARG_LENGTH); //get the cmd_name... not needed
length += get_word(arg1, (args+length), ARG_LENGTH);
if (!*arg1) {
tprintf("wrong args\n");
return;
}
length = get_word(arg2, (args+length), ARG_LENGTH);
if (!*arg2) {
tprintf("wrong args\n");
return;
}
int servo_nr = atoi(arg1);
int servo_val = atoi(arg2);
tprintf("set servo nr %i to %i \n", servo_nr, servo_val);
servo_set(servo_val, servo_nr);
}
void main(void)
{
unsigned char index[3];
unsigned int position=0;
int error=0;
//Declaration related to PID control systems.
int iterm=0;
unsigned int servo_output=1500;
float correction;
Leds_and_Switches_Init();
ISR_init();
ATD_init();
dc_motor_init(); // Current pwm duty is 50
servo_init();
//uart0_9600_init(); //To be removed in the final version of the software.
for(;;)
{
//uart0_tx_string(" :::::::::::::: I'm now in main Loop !! :::::::::::: ");
//This function compensates the sen_data array and puts the result into com_data array
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
if(com_data[0] <=5 && com_data[1] <=5 && com_data[2] <=5 && com_data[3] <=5 && com_data[4] <=5 && com_data[5] <=5 && com_data[6] <=5 && com_data[7] <=5)
{
//The flag (ATDSTAT0_SCF=0;) clears automatically when we access status register and then the result register..
//dc_motor_stop();
}
/* else if(com_data[0] >=240 && com_data[1] >=240 && com_data[2] >=240 && com_data[3] >=240 && com_data[4] >=240 && com_data[5] >=240 && com_data[6] >=240)
{
}
*/
else
{
sort_array(com_data,index);
position = (index[1] * 512) + (com_data[0]-com_data[2]) ;
error = 1536 - position;
//PID
// limits bound on iterm.
if (iterm>=30000) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = 30000;
LED3=~LED3;
}
else if (iterm<=-30000) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -30000;
LED4=~LED4;
}
else
{
iterm += error/20 ;
}
correction = (kp*error) + (ki *iterm) ;
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
// -ve correction means -ve error, ie turn right
// +ve correction means +ve error ie turn left.
servo_output = 1500-(unsigned int)correction;
servo_set (servo_output);
}
} //for loop end here
}
int servo_speed(int pin, int speed) // Set continuous rotation speed
{
return servo_set(pin, speed + 1500); // Add center pulse width to speed
}
int servo_angle(int pin, int degreeTenths) // Set continuous rotation speed
{
return servo_set(pin, degreeTenths + 500); // Add center pulse width to speed
}
void closeWindow(servo_t *servo) {
servo_set(servo, STEP_UPPER_BOUND);
}
