void repeatAlways(void)
{
printf("%s%d\n","Thermometer reading is: ",analog(thermometer));
set_servo(leftServo, 15);
set_servo(rightServo, 75);
printf("%s%d\n","Temperature in Celcius is: ",(((analog(thermometer) - 32) * 5) /(float) 9));
}
void BuclePrincipal()
{
if(flag_10hz)
{
flag_10hz=0;
atc_parser();
Copy2DatosAntena();
//CalculateHeading();
CalculaPanTilt();
CheckeaPulsaciones();
if((debugInfo.EnableDebug&0x03) == 0)
{
set_servo_pan(pan+offset_pan);
}
else if((debugInfo.EnableDebug&0x03) == 1)
{
set_servo(SERVO_PAN,debugInfo.pan);
}
else
set_servo_pan(debugInfo.grados_pan+offset_pan);
if((debugInfo.EnableDebug&0x0C) == 0)
{
set_servo_tilt(tilt);
}
else if((debugInfo.EnableDebug&0x0C) == 4)
{
set_servo(SERVO_TILT,debugInfo.tilt);
}
else
set_servo_tilt(debugInfo.grados_tilt);
if(!datosAntena.v_bateria_alarm)
{
LED=1;
}
else if(tics&0x40)
{
LED=0;
}
else
{
LED=1;
}
}
}
int main(void){
servos_init();
uint8_t servo_pin=2;
add_servo(servo_pin);
for(int i=1200;i<1800;i++){
set_servo(servo_pin,i);
_delay_ms(10);
}
set_servo(servo_pin,0);
return 0;
}
/* Set and remember power level */
void alter_power(int err, int motor_index){
int tmp_pwr = 0; // Temporary servo power value
tmp_pwr = ((mPower[motor_index] * CLICKS) + err) / CLICKS;
mPower[motor_index] = limit_range(tmp_pwr, -200, 200);
set_servo(mPower[motor_index], motor_index);
}
int main (void)
{
//Set PORTC to all outputs
DDRB = 0xFF;
//create an infinite loop
while(1) {
int j=0;
for(j=0;j<15*50;j++){
set_servo(9);
}
for(j=0;j<50;j++){
set_servo(10);
}
}
return 1;
}
void motor(uint8_t num, int8_t speed) {
if(num == RIGHT_SERVO)
speed = speed/100.0 * MAXSPEEDRANGE + 127;
else
speed = -1*speed/100.0 * MAXSPEEDRANGE + 127;
set_servo(num, speed);
}
void test_outputs(void)
{
set_servo(SERVO1,0);
set_servo(SERVO2,0);
_delay_ms(500);
set_servo(SERVO1,0xFF);
set_servo(SERVO2,0xFF);
_delay_ms(1000);
set_servo(SERVO1,0);
_delay_ms(1000);
set_servo(SERVO2,0);
_delay_ms(1000);
VALVE1_ON;
LED_VAVLE1_ON;
_delay_ms(500);
VALVE2_ON;
LED_VAVLE2_ON;
_delay_ms(500);
LED_ALERT_ON;
BUZZER_ON;
_delay_ms(500);
VALVE1_OFF;
VALVE2_OFF;
LED_VAVLE1_OFF;
LED_VAVLE2_OFF;
LED_ALERT_OFF;
BUZZER_OFF;
}
static ssize_t dev_write(struct file *filp,const char *user_buf,size_t count,loff_t *f_pos)
{
struct private_data* const pdata = filp->private_data;
char buf[128], *p = buf, nl;
int len = pdata->partial_len;
if (0 == pdata)
return -EFAULT;
if (pdata->reject_writes)
return -EINVAL;
memcpy(buf, pdata->partial_cmd, len);
pdata->partial_len = 0;
if (count > sizeof(buf) - len - 1)
count = sizeof(buf) - len - 1;
if (copy_from_user(buf+len, user_buf, count))
return -EFAULT;
len += count;
buf[len] = '\0';
while (p < buf+len) {
int servo, cnt, res;
if (strchr(p, '\n')) {
if (sscanf(p, "%d=%d%c", &servo, &cnt, &nl) != 3 ||
nl != '\n') {
printk(KERN_WARNING "ServoBlaster: Bad data format\n");
pdata->reject_writes = 1;
return -EINVAL;
}
res = set_servo(servo, cnt);
if (res < 0) {
pdata->reject_writes = 1;
return res;
}
p = strchr(p, '\n') + 1;
}
else if (buf+len - p > 10) {
printk(KERN_WARNING "ServoBlaster: Bad data format\n");
pdata->reject_writes = 1;
return -EINVAL;
}
else {
// assume more data is coming...
break;
}
}
pdata->partial_len = buf+len - p;
memcpy(pdata->partial_cmd, p, pdata->partial_len);
return count;
}
static void angle_control(angle_t err)
{
static double errs[2]={0};
errs[1]=errs[0];
errs[0]=err;
if(abs(err)<2&&fabs(errs[0]-errs[1])<2)
{
return;
}
double P,D;
int32 result;
P=(err*err)*1.1+20;
D=P/3;
result=(int32)(err*P+D*(errs[0]-errs[1])+0.5);
if(result>=0)
{
set_servo(servo_right,result);
}
else
{
set_servo(servo_left,-result);
}
}
void InitAntTracker()
{
pageRAMaddr=0;
LED = 1;
CheckConfig();
tics=0;
set_servo(SERVO_PAN,1500);
set_servo(SERVO_TILT,1500);
// set_servo(SERVO_AUX,1500);
UpdatedDatosAvion=0;
datosAvion.lon = -6.0f;
datosAvion.lat = 37.0f;
datosAvion.alt = 0.0f;
datosAvion.home_lon = -6.0f;
datosAvion.home_lat = 37.0f;
datosAvion.home_alt = 0.0f;
debugInfo.EnableDebug = 0;
debugInfo.pan = 1500;
debugInfo.tilt = 1500;
debugInfo.grados_pan = 0.0f;
debugInfo.grados_tilt = 0.0f;
InitUSB();
InitPWM();
offset_pan = 0.0f;
AD0BUSY = 1;
}
int main()
{
/******************************************
Peripheral initialization
gpio and i2c PCA9685
Gain access to peripheral memory structures
******************************************/
map_peripheral_BCM2835(&gpio);
map_peripheral_BCM2835(&bsc0);
init_I2C_protocol();
// Board setup
init_PCA9685(SERVOHATADDR);
set_PWM_frequency_PCA9685(SERVOHATADDR, SERVO_FREQUENCY);
init_angle_to_pulse_length_lookup_table();
printf("Set Servo Driver...\n");
//set_servo(SERVOHATADDR, 0 , SERVO_FREQUENCY, 120);
//set_servo(SERVOHATADDR, 1 , SERVO_FREQUENCY, 120);
set_servo(SERVOHATADDR, 2 , SERVO_FREQUENCY, 150);
return 0;
}
// Close Serial Port
void
close_serial()
{
// printf("Not Releasing channels (Commented out)\n");
printf("Setting servo and motor to 1500");
set_servo(1500);
set_motor(1500);
printf("Releasing channels");
release_channels();
printf("CLOSE PORT\n");
int result = close(fd);
if ( result )
{
fprintf(stderr,"WARNING: Error on port close (%i)\n", result );
}
status = 0;
printf("\n");
}
//TESTING
void test_servo() //test servo
{
set_servo(CENTER); delay_ms(2000);
set_servo(MAX_LEFT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
set_servo(MAX_RIGHT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
set_servo(MIN_RIGHT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
set_servo(MAX_RIGHT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
set_servo(MIN_LEFT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
set_servo(MAX_LEFT); delay_ms(2000);
set_servo(CENTER); delay_ms(2000);
};
void device_init()
{
unsigned char i,*msg="G-Bot v3\n Hello!";
//ports init
PORTB=0x00;DDRB=0x0F;
DDRC=0x20;PORTC=0x18; //free
PORTD=0x00;DDRD=0x00; //free | LCD
//PORTC=0x00;DDRC=0x44;PORTD=0x00;DDRD=0x1F; //LEDS
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
TCCR0=0x00;TCNT0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;TCCR1B=0x00;TCNT1H=0x00;TCNT1L=0x00;ICR1H=0x00;
ICR1L=0x00;OCR1AH=0x00;OCR1AL=0x00;OCR1BH=0x00;OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 15,625 kHz
// Mode: Fast PWM top=FFh
// OC2 output: Non-Inverted PWM
ASSR=0x00;TCCR2=0x00;TCNT2=0x00;OCR2=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
MCUCR=0x00;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;SFIOR=0x00;
// ADC initialization
// ADC Clock frequency: 500,000 kHz
// ADC Voltage Reference: AVCC pin
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0x85;
//starting values
set_servo(CENTER);
//init was complete
for(i=0;i<3;i++) {LED=1;delay_ms(80);LED=0;delay_ms(80);};
// LCD module initialization
lcd_init(8);
lcd_gotoxy(0,0); // put LCD cursor to start position
lcd_puts(msg); // print str on LCD
};
////////////////////////////////////////////////
//BOT CONTROL
void bot_control(void)
{
//S0
if(SH_LEFT>=125 && SH_LEFT<=140)
{
S4_t=0; //for S4
CURR_S=S0;
if(!S3_t) set_servo(CENTER);
if(SH_CENTER<70 && SH_CENTER>=55) {speed_down(4);goto f_exit;}
else if(SH_CENTER>=70) {S3_t=1;speed_down(4);set_servo(MAX_RIGHT);goto f_exit;}
else if(SH_CENTER<55) {S3_t=0;speed_up(5);goto f_exit;}
}
//S1
else if(SH_LEFT>140 && SH_LEFT<=240)
{
S4_t=0; //for S4
CURR_S=S1;
//-------------------
if(SH_CENTER>=70)
{
S3_t=1; //for S3
speed_down(4);
set_servo(MAX_RIGHT);
goto f_exit;
}
else
{
S3_t=0;
set_servo(MIN_RIGHT);
speed_down(2);
}
//-------------------
goto f_exit;
}
//S2
else if(SH_LEFT>240 || S3_t)
{
S4_t=0; //for S4
CURR_S=S2;
//-------------------
set_servo(MAX_RIGHT);
speed_down(5);
//-------------------
goto f_exit;
}
//S3
else if(SH_LEFT<125 && SH_LEFT>=100 && !S3_t)
{
S4_t=0; //for S4
CURR_S=S3;
//-------------------
set_servo(MIN_LEFT);
speed_down(3);
//-------------------
goto f_exit;
}
//S4
else if(SH_LEFT<100)
{
S3_t=0;
//-------------------
switch(S4_t)
{
case 0:
{
S4_t=1;
CURR_S=S40;
speed_down(3);
set_servo(MIN_LEFT);
delay_ms(10);
break;
};
case 1:
{
CURR_S=S41;
speed_down(1);
set_servo(MAX_LEFT);
break;
};
};
//-------------------
goto f_exit;
}
f_exit:
#asm
nop
#endasm
};
void main(void)
{
device_init();
//START
START_MOTOR;
FORWARD;
set_speed(SPEED_MIN);
while (1)
{
//BREAKERS
if(LEFT_BREAKER)
{
//-----------------------
BACKWARD;
set_speed(SPEED_BACKWARD);delay_ms(50);
set_servo(MAX_LEFT);delay_ms(150);
STOP;
set_servo(MAX_RIGHT);delay_ms(100);
FORWARD;
set_speed(SPEED_MIN);delay_ms(20);
continue;
//-----------------------
};
if(RIGHT_BREAKER)
{
//-----------------------
BACKWARD;
set_speed(SPEED_BACKWARD);delay_ms(50);
set_servo(MAX_RIGHT);delay_ms(150);
STOP;
set_servo(MAX_LEFT);delay_ms(100);
FORWARD;
set_speed(SPEED_MIN);delay_ms(20);
continue;
//-----------------------
};
//IR_SENSOR
if(IR_SENSOR)
{
BACKWARD;
set_speed(SPEED_BACKWARD);delay_ms(30);
set_servo(MAX_LEFT);delay_ms(70);
STOP;
set_servo(MAX_RIGHT);delay_ms(100);
FORWARD;
set_speed(SPEED_MIN);delay_ms(20);
continue;
};
//reading Sharp2D120X
read_sensors();
////////////////////////////////////////////////
//---------------------------------------------
//bot control
bot_control();
///////////////////////////////////////////////
//LCD
to_lcd();
};
}
void thread_maintain_database(void * thread_id){
//recevie control terminal and maintan databse
struct data * raw_curr;
struct data * raw_prev = NULL;
//struct servo * servo_curr;
printf("maintan thread is created\n");
char data[BUFFER_SIZE];
//inital set up to all middle positions
set_servo(SERVOHATADDR, 6 , SERVO_FREQUENCY, 90);
set_servo(SERVOHATADDR, 2 , SERVO_FREQUENCY, 155);
set_servo(SERVOHATADDR, 0 , SERVO_FREQUENCY, 155);
set_servo(SERVOHATADDR, 9, SERVO_FREQUENCY, 155);
set_servo(SERVOHATADDR, 4 , SERVO_FREQUENCY, 180);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 0);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 0);
while(1){
raw_curr = raw_head;
if(raw_curr != NULL){
pthread_mutex_lock(&raw_data_lock);
if(raw_head != NULL){
raw_curr = raw_head;
while(raw_curr->next != NULL){
raw_curr = raw_curr->next;
raw_prev = raw_curr;
}
if(raw_prev == NULL){
strcpy(data, raw_curr->data);
free(raw_curr->data);
free(raw_curr);
raw_head = NULL;
}
else{
strcpy(data, raw_curr->data);
raw_curr->prev->next = NULL;
free(raw_curr->data);
free(raw_curr);
}
char *array[10];
int i=0;
array[i] = strtok(data,":");
while(array[i]!=NULL && i < 10)
{
array[++i] = strtok(NULL,":");
}
if (!strcmp(array[0], "1")){
int baseServo = atoi(array[1]);
int shoulderServo = atoi(array[2]);
int elbowServo = atoi (array[3]);
int handRoll = atoi(array[4]);
int rollServo = 155+handRoll;
if(rollServo > 250){
rollServo = 250;
}else if (rollServo < 20){
rollServo = 20;
}
double claw = atof(array[5]);
int clawServo;
if(claw < 0.5)
{
clawServo = 50;
}else if (claw == 0.5){
clawServo = 125;
}else{
clawServo = 180;
}
printf("baseServo: %d, shoulderServo: %d, elbowServo: %d, rollServo: %d\n", baseServo, shoulderServo, elbowServo, rollServo);
set_servo(SERVOHATADDR, 6 , SERVO_FREQUENCY, baseServo);
set_servo(SERVOHATADDR, 2 , SERVO_FREQUENCY, shoulderServo);
set_servo(SERVOHATADDR, 0 , SERVO_FREQUENCY, elbowServo);
set_servo(SERVOHATADDR, 9, SERVO_FREQUENCY, rollServo);
set_servo(SERVOHATADDR, 4 , SERVO_FREQUENCY, clawServo);
}else if (!strcmp(array[0], "2")){
int handRoll = atoi(array[1]);
int rollServo = 155+handRoll;
double claw = atof(array[2]);
int clawServo;
if(claw < 0.5){
clawServo = 50;
}else if (claw == 0.5){
clawServo = 125;
}else{
clawServo = 180;
}
if(rollServo > 250){
rollServo = 250;
}else if (rollServo < 20){
rollServo = 20;
}
printf("rollServo: %d, clawServo: %d\n", rollServo, clawServo);
set_servo(SERVOHATADDR, 9 , SERVO_FREQUENCY, rollServo);
set_servo(SERVOHATADDR, 4 , SERVO_FREQUENCY, clawServo);
}else if (!strcmp(array[0], "3")){
int M1Direction = atoi(array[1]);
int M2Direction = atoi(array[2]);
if(M1Direction == 0){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_ST);
printf("M1 Stop");
}else if(M1Direction == 1){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_FW);
printf("M1 Forward");
}else if(M1Direction == 2){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_BW);
printf("M1 Backward");
}
if(M2Direction == 0){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_ST);
printf("M2 Stop");
}else if(M2Direction == 1){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_FW);
printf("M2 Forward");
}else if(M2Direction == 2){
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_BW);
printf("M2 Backward");
}
}
/*
if(hand_x_position > 90)
hand_x_position = 90;
else if(hand_x_position < -90)
hand_x_position = -90;
if(hand_y_position > 90)
hand_y_position = 90;
else if (hand_y_position < -90)
hand_y_position = -90;
hand_y_position += 90;
if(hand_z_position > 70)
hand_z_position = 70;
if (hand_z_position < -60)
hand_z_position = -50;
hand_z_position += 80;
printf("pm_degree: %d\n", pm_degree);
printf("ip_degree: %d\n", ip_degree);
printf("hand_x_position: %d\n", hand_x_position);
printf("hand_y_position: %d\n", hand_y_position);
printf("hand_z_position: %d\n", hand_z_position);
printf("roll_degree: %d\n", roll_degree);
printf("direction: %d\n", direction);
if(hand_y_position >= 30 && hand_y_position <= 180){
if(hand_z_position >= 0 && hand_z_position < 80){
knee_2 = 95;
knee_1 = 100;
}
else if(hand_z_position >= 80 && hand_z_position <= 150){
knee_2 = 120;
knee_1 = 120;
}
}
else if(hand_y_position >= 0 && hand_y_position < 30){
if(hand_z_position >= 0 && hand_z_position < 80){
knee_2 = 105;
knee_1 = 90;
}
else if(hand_z_position >= 80 && hand_z_position <= 150){
knee_2 = 135;//120
knee_1 = 120;
}
}
if(hand_x_position >= 90){
hand_x_position = 90;
}
else if (hand_x_position < 0){
if(hand_x_position < - 90)
hand_x_position = -90;
}
hand_x_position = hand_x_position + 90;
if(roll_degree > 90)
roll_degree = 90;
else if (roll_degree < -90)
roll_degree = -90;
roll_degree += 90;
if(pm_degree >= 10)
pm_degree += 20;
else if(pm_degree >= 20)
pm_degree += 30;
else if(pm_degree >= 30)
pm_degree += 40;
else if(pm_degree >= 40)
pm_degree += 50;
else if(pm_degree >= 50)
pm_degree += 60;
else if(pm_degree >= 60)
pm_degree += 70;
else if(pm_degree >= 70)
pm_degree += 80;
else if (pm_degree >= 80)
pm_degree += 90;
else if(pm_degree <= 0)
pm_degree += 10;
*/
//printf("knee1=%d,knee2=%d,hand_z=%d\n",knee_1,knee_2,hand_z_position);
//set_servo(SERVOHATADDR, 0 , SERVO_FREQUENCY, knee_1);
//set_servo(SERVOHATADDR, 1 , SERVO_FREQUENCY, knee_2);
//set_servo(SERVOHATADDR, 2 , SERVO_FREQUENCY, hand_z_position);
/*
switch(direction){
case 1:
printf("case1/n");
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_FW);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_FW);
break;
case 2:
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_BW);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_BW);
break;
case 3:
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_BW);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_FW);
break;
case 4:
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_FW);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 255);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_BW);
break;
case 5:
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 0);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_ST);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 0);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_ST);
break;
default:
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, 0);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_1, DIRECTION_ST);
set_speed_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, 0);
set_direction_DC_motor(MOTORADDRESS, MOTOR_CHANNEL_2, DIRECTION_ST);
break;
}*/
bzero(data, BUFFER_SIZE);
pthread_mutex_unlock(&raw_data_lock);
}
}
raw_curr = NULL;
raw_prev = NULL;
}
pthread_exit(NULL);
}
int main(void) {
int i;
int reading1;
int reading2;
int address;
int test_array[100];
for(i=0;i<100;i++) {
motor(0,i); //spin the left motor forward
motor(1,i); //spin the right motor forward
}
i=0;
while(i>-100) {
motor(0,i); //spin the left motor backwards
motor(1,i); //spin the right motor backwards
i--;
}
i=50;
set_servo(0,i); //set servo motor 0 to move to 50 degrees
set_servo(3,i); //set servo motor 3 to move to 50 degrees
delay_milliseconds(100); //pause 100 milliseconds
delay_seconds(1); //pause 1 second
lcd_clear();
lcd_cursor(0,0);
printf ("Test1\n"); //the LCD will be 8x2 (8chars x 2lines)
printf ("Test2\n");
reading1 = analog(0); //get a reading from analog pin 0
reading2 = analog(5); //get a reading from analog pin 5
reading1 = digital(0); //get a reading from digital pin 0
reading2 = digital(1); //get a reading from digital pin 1
if (reading1 > 100) {
printf ("%d\n", reading1);
}
reading1 = accelerometer(0); //read x-axis
reading2 = accelerometer(1); //read y-axis
reading1 = accelerometer(2); //read z-axis
reading1 = battery_voltage(); //battery voltage
reading1 = read_serial_port(); //get a byte from the serial port
write_serial_port(reading1); //send a byte on the serial port
led1(1); //turn on on-board led1
led1(0); //turn off on-board led1
reading1 = read_ir(); //get a reading from the IR receiver
reset(); //reset the board
write_eeprom(address, reading1); //write a value to the non-volatile eeprom (these values will be stored across resets)
reading1 = read_eeprom(address); //get a reading from the non-volatile eeprom
reading1 = button(); //read the state of the on-board button
return 0;
}
//------------------------------------------------------------------------------
void pic_MainLoop()
{
// Initialize connections
fprintf(stderr,"Init USB... ");
fflush(stdout);
if(init_analog_in(NB_ANALOGS) < 0)
{
fprintf(stderr,"INIT ANALOG FAILED !\n");
pic_exit(1);
return;
}
else
{
fprintf(stderr,"ok\n");
fflush(stdout);
struct timeval time;
double curr_time;
double init_time = time.tv_sec + time.tv_usec*0.000001;
gettimeofday(&time,NULL);
while(!stop_loop)
{
pic_ready = true;
while(!stop_loop)
{
digital_output = get_digital_in();
if(digital_output < 0)
{
fprintf(stderr,"Erreur get_digital_in\n");
break;
}
// Jack
if(!strat_is_started() && (digital_output & DIGIT_JACK))
strat_lets_go();
// Coders
int coder_left, coder_right;
unsigned char dir_left, dir_right;
int iter = get_codeuses(&coder_left, &dir_left, &coder_right, &dir_right);
if(iter == -1)
{
fprintf(stderr,"Erreur get_codeuses\n");
break;
}
else
{
gettimeofday(&time,NULL);
curr_time = time.tv_sec + time.tv_usec*0.000001;
callbackRecvCoder(curr_time-init_time, -coder_left, coder_right);
}
// Motors
if(change_power)
{
change_power = 0;
if(set_speed(int(127.*power_left)+128, int(127.*power_right)+128) != 1)
{
fprintf(stderr,"Erreur set_speed\n");
break;
}
}
// Pusher
if(move_pusher)
{
if(set_DC_motor(MOTOR_PUSHER_LEFT, where_pusher) == 1)
{
if(set_DC_motor(MOTOR_PUSHER_RIGHT, where_pusher) == 1)
move_pusher = false;
else
{
set_DC_motor(MOTOR_PUSHER_LEFT, MOTOR_PUSHER_STOP);
break;
}
}
else
break;
}
// Door
if(move_door)
{
if(set_servo(SERVOM_DOOR, where_door) == 1)
move_door = false;
else
break;
}
// Distance captors
get_analog_in(captors, 4);
#ifdef SIMULATION
usleep(TIMER_CODER);
#endif
}
if(!stop_loop)
{
pic_ready = false;
while(repare_usb() != 1) {}
}
}
}
}
void main() {
byte SERVO_0 = 0, SERVO_1 = 0;
byte MOTOR_L = 0, MOTOR_R = 0, MOTOR_V = 0;
short BUCKET_L = 0, BUCKET_R = 0, BUCKET_V = 0;
byte T = 0;
init();
while (1) {
byte cmd = serial_rx();
byte power, s0, s1;
switch (cmd) {
case CMD_SET_MOTORS :
MOTOR_L = serial_rx();
MOTOR_R = serial_rx();
MOTOR_V = serial_rx();
s0 = serial_rx();
s1 = serial_rx();
set_servo(1 << 0, SERVO_0);
set_servo(1 << 1, SERVO_1);
SERVO_0 = s0;
SERVO_1 = s1;
// read temperature data from RE2 (ANS7)
serial_tx(read_analog(7));
break;
}
BUCKET_L += MOTOR_L - 127;
BUCKET_R += MOTOR_R - 127;
BUCKET_V += MOTOR_V - 127;
power = 0;
if (BUCKET_L >= 127) {
BUCKET_L -= 127;
power = power | (1 << 0);
}
else if (BUCKET_L < -127) {
BUCKET_L += 127;
power = power | (1 << 1);
}
if (BUCKET_R >= 127) {
BUCKET_R -= 127;
power = power | (1 << 2);
}
else if (BUCKET_R < -127) {
BUCKET_R += 127;
power = power | (1 << 3);
}
if (BUCKET_V >= 127) {
BUCKET_V -= 127;
power = power | (1 << 4);
}
else if (BUCKET_V < -127) {
BUCKET_V += 127;
power = power | (1 << 5);
}
PORTA = power;
wait_msec(4);
}
}
int main(void)
{
lcd_init(LCD_DISP_ON);
lcd_clrscr();
lcd_puts("Je suis");
lcd_gotoxy(0,1);
lcd_puts("Roberto");
int i, pos_servo, flag_error = 0;
DDRC = 0xFF;
DDRD = 0xFF;
DDRA = 0x80;
PORTD = 0b01100011;
PORTC = 0b00000000;
set_servo(0, 20);
set_servo(1, 80);
set_servo(2, 50);
set_servo(3, 50);
set_servo(4, 50);
while(1)
{
/*
if(_PINA0 == 1 )
{
if(_PINA1 == 0)
{
_PORTA7 = 1;
}
else _PORTA7 = 0;
}
else _PORTA7 = 0;
*/
/*
_PORTA7 = 1;
waitms(3000);
_PORTA7 = 0;
waitms(5000);
*/
/*
if(_PINA5 == 1 )
{
set_servo(3, 20);
}
else set_servo(3, 50);
*/
if(_PINA0 == 1)
{
sortie: PORTD = 0b11000110;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Payez le gentil");
lcd_gotoxy(0,1);
lcd_puts("serveur SVP");
while(_PINA5 == 0);
PORTD = 0b00100001;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Et un Ricard");
for(i=0; i < 69; i++)
{
set_servo(0, 20 + i);
waitms(20);
}
i = 0;
while(_PINA2 == 0)
{
set_servo(1, 80 - i);
waitms(20);
i++;
}
pos_servo = i;
while(_PINA0 == 1 && _PINA2 == 1 && _PINA1 == 0) _PORTA7 = 1;
if(_PINA2 == 1 && _PINA0 == 1)
{
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 50);
flag_error = 0;
}
else
{
PORTD = 0b10000100;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Nous avons");
lcd_gotoxy(0,1);
lcd_puts("un probleme.");
flag_error = 1;
}
_PORTA7 = 0;
waitms(5000);
for(i=0; i < 70; i++)
{
set_servo(1, 10 + i);
waitms(30);
}
if(flag_error == 0)
{
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Et voila.");
PORTD = 0b01000010;
}
while( _PINA0 == 1)
{
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
waitms(200);
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
waitms(2000);
}
for(i=0; i < 70; i++)
{
set_servo(0, 85 - i);
waitms(30);
}
if(flag_error == 0)
{
lcd_clrscr();
lcd_gotoxy(0,0);
lcd_puts("A votre sante.");
}
waitms(2000);
}
else
{
PORTD = 0b01100011;
//set_servo(0, 20);
//set_servo(1, 80);
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Je suis");
lcd_gotoxy(0,1);
lcd_puts("Roberto");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Le robot");
lcd_gotoxy(0,1);
lcd_puts("serveur");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Je fais parti");
lcd_gotoxy(0,1);
lcd_puts("de Robopoly");
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Un Ricard");
lcd_gotoxy(0,1);
lcd_puts("Un vrai.");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
}
/*
if(_PINA2 == 0)
{
waitms(200);
set_servo(1, 80);
}
else
{
waitms(200);
set_servo(1, 20);
}
*/
/*
set_servo(0, 30);
set_servo(1, 30);
set_servo(2, 30);
set_servo(3, 40);
set_servo(4, 40);
waitms(500);
set_servo(0, 70);
set_servo(1, 70);
set_servo(2, 70);
set_servo(3, 60);
set_servo(4, 60);
waitms(500);
*/
/*
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000100;
waitms(20-i+1);
PORTD = 0b00000001;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000001;
waitms(20-i+1);
PORTD = 0b00000010;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000010;
waitms(20-i+1);
PORTD = 0b00000100;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
*/
}
return 0;
}
