/*void follow_wall()
{
set_speed(0);
while (true)
{
take_picture();
bool seen_wall = false;
for (int i = -100; i < 100; i++) { for (int j = -50; j <= 50; j++) {
int pixel = get_pixel(IMAGE_SIZE_X / 2 + i, IMAGE_SIZE_Y / 2 + j, COLOR_WHITE);
if (pixel >= WHITE_THRESHOLD) {
seen_wall = true;
goto found_wall;
}
}}
found_wall:
if (seen_wall) {
set_speed(20);//20
turn(75);//-80
} else {
set_speed(30);//30
turn(-75);//80
}
}
halt();
}
*/
void follow_wall(){
set_speed(50);
turn(0);
while(true){
frontVal = read_analog(FRONT_PIN);
leftVal = read_analog(LEFT_PIN);
rightVal = read_analog(RIGHT_PIN);
while(leftVal < (rightVal+40) && leftVal > (rightVal-40)){
set_speed(50);
}
while(leftVal > (rightVal+40)){
set_speed(40);
turn(10);
Sleep(0, 500);
}
while(leftVal < (rightVal+40)){
set_speed(40);
turn(-10);
Sleep(0, 500);
}
if(frontVal > 550){
if(rightVal < 300){
turn(1 * SHARP_TURN);
Sleep(0, TURN_WAIT);
}
if(leftVal < 300){
turn(-1 * SHARP_TURN);
Sleep(0, TURN_WAIT);
}
}
}
}
void maze(){
// maze code
int prev_error = 0;
int total_error = 0;
while(true){
int ir_sensor_left = read_analog(2);
int ir_sensor_right = read_analog(1);
int ir_sensor_forward = read_analog(0);
printf("Left:%d\n", ir_sensor_left);
printf("Right:%d\n", ir_sensor_right);
printf("Ford:%d\n", ir_sensor_forward);
int error_signal = ir_sensor_right - ir_sensor_left;
total_error += error_signal;
// see PID control https://github.com/kaiwhata/ENGR101-2016/wiki/PID-(Proportional-Integral-Derivative)-Control
double proportional_signal = error_signal*MAZE_KP;
double derivative_signal = (error_signal-prev_error/0.1)*MAZE_KD;
double integral_signal = total_error*MAZE_KI;
prev_error = error_signal;
int total_signal = proportional_signal + derivative_signal + integral_signal;
// robot will always turn right
if(ir_sensor_right < 130){
printf("hi\n");
set_motor(1, MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
Sleep(0, 800000);
set_motor(1, MOTOR_SPEED);
set_motor(2, 0);
Sleep(1,500000);
ir_sensor_right = read_analog(2);
/*if(ir_sensor_right < 130){
set_motor(1, MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
Sleep(0, 500000);
set_motor(1, MOTOR_SPEED);
set_motor(2, 0);
Sleep(1,0);
}*/
} else if(ir_sensor_forward > 300){
// spins robot around
set_motor(1, -MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
} else {
// goes straight forward
set_motor(1, MOTOR_SPEED - total_signal);
set_motor(2, -MOTOR_SPEED - total_signal);
}
Sleep(0,SLEEP_TIME);
}
return;
}
int main(){
init(1);
int x;
connect_to_server("130.195.6.196", 1024); //Opening the gate code
send_to_server("Please");
char message[24];
receive_from_server(message);
printf("%s", message);
send_to_server(message);
for (x = 0; x < 8; x++)
{
select_IO(x,0);
write_digital(x,1);
}
while(1){
take_picture();
int sum = 0;
float kp = 0.5;
int w;
int s;
int proportional_signal=0;
for(int x = 0; x < 320; x++){
w = get_pixel(x,120,3);
if(w>127){s=1;}
else{s=0;};
sum = sum + (x-160)*s;}
proportional_signal = sum * kp;
if (sum < 0){
set_motor(1, proportional_signal);
set_motor(2, -1*proportional_signal);
}
else if (sum > 0){
set_motor(1, -1*proportional_signal);
set_motor(2, proportional_signal);
}
else{
set_motor(1, proportional_signal);
set_motor(2, proportional_signal);
}
update_screen();
for (x = 0 ; x < 8; x++)
{
int adc_reading = read_analog(x);
printf("ai=%d av=%d\n",x,adc_reading);
}
}
set_motor(1,0);
set_motor(2,0);
return 0;
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
init(0);
//We declare an integer variable to store the ADC data
int adc_readingfront;
int adc_readingfrontleft;
int adc_readingbackleft;
while(true){
adc_readingfront = read_analog(0);
adc_readingfrontleft = read_analog(1);
adc_readingbackleft = read_analog(2);
printf("Front: %d\n",adc_readingfront);
printf("Front Left: %d\n",adc_readingfrontleft);
printf("Back Left: %d\n\n",adc_readingbackleft);
}
}
void dump_sensors(uint16_t digital, uint8_t analog) {
uint8_t pin;
uint8_t value;
while(1) {
if (USART_RXBufferData_Available(&BT_data) && '*' == USART_RXBuffer_GetByte(&BT_data))
return;
bt_putchar(':');
for (pin=0; pin<10; pin++)
if (digital & (1<<pin))
bt_putchar(read_input('0'+pin)+48);
for (pin=0; pin<6; pin++)
if (analog & (1<<pin)) {
value = read_analog(pgm_read_byte_near(muxPosPins+pin), 0);
put_hex_nibble(value>>4);
put_hex_nibble(value&0xF);
}
_delay_ms(5);
}
int main()
{
init_timer_0();
init_pin_interrupt(PCINT0);
init_adc();
util_init();
// Outputs
DDRB |= (1 << PB1);
DDRB |= (1 << PB3);
DDRB |= (1 << PB4);
PORTB &= ~(1 << PB3); // Set LED low
PORTB &= ~(1 << PB1); // Set Servo pin low
// Inputs
DDRB &= ~(1 << PB0); /* Set PB0 as input */
PORTB |= (1 << PB0); /* Activate PULL UP resistor */
// Toggle LED
PORTB ^= (1 << PB3);
_delay_ms(1000);
PORTB ^= (1 << PB3);
// Enable global interrupts
sei();
while(1)
{
reading = read_analog();
reading_map = map(reading, 0, 255, 410, 1580, 1) / 1000.0 / 0.01;
}
}
void main()
{
struct rx_stat rxstat;
int32 rx_id;
int in_data[8];
int rx_len;
int out_data[8];
int16 tmp;
int32 tx_id;
int1 tx_rtr=0;
int1 tx_ext=0;
int tx_len=8;
int tx_pri=3;
// bitmap, expanded to 8 bit (needed?)
int8 sequence[MAX_CHANNELS];
int32 val;
int i;
for (i = 0; i < MAX_CHANNELS; i++)
sequence[i] = MAX_CHANNELS;
// resource initialization.
setup_adc_ports(AN0);
setup_adc(ADC_CLOCK_INTERNAL);
setup_spi(FALSE);
setup_wdt(WDT_OFF);
setup_timer_0(RTCC_INTERNAL);
setup_timer_1(T1_DISABLED);
//setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
//set_timer1(16000);
setup_timer_2(T2_DISABLED,0,1);
setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
_board_ID = read_eeprom(0);
can_init();
adc_init();
enable_interrupts(INT_TIMER1);
enable_interrupts(GLOBAL);
while(TRUE)
{
if (can_kbhit() || _wait) // wait for a message on the CAN bus
{
// handles timer message
if (_wait)
{
_wait = 0;
for (i = 0; i <= MAX_CHANNELS-3; i+=3)
{
if (sequence[i] < MAX_CHANNELS)
tmp = read_analog(i);
else
tmp = 0;
out_data[1] = (int8)(tmp);
out_data[2] = (int8)(tmp>>8);
if (sequence[i+1] < MAX_CHANNELS)
tmp = read_analog(i+1);
else
tmp = 0;
out_data[3] = (int8)(tmp);
out_data[4] = (int8)(tmp>>8);
if (sequence[i+2] < MAX_CHANNELS)
tmp = read_analog(i+2);
else
tmp = 0;
out_data[5] = (int8)(tmp);
out_data[6] = (int8)(tmp>>8);
while (!can_tbe()) ;
tx_id = ID_BASE;
tx_id |= ((_board_ID) << 4);
//tx_id |= ((0 & 0x00f0) >> 4);
out_data[0] = 0x30+i;
tx_len = 7;
can_putd(tx_id, out_data, tx_len, tx_pri, tx_ext, tx_rtr);
}
// last message.
if (sequence[30] < MAX_CHANNELS)
tmp = read_analog(30);
else
tmp = 0;
out_data[1] = (int8)(tmp);
out_data[2] = (int8)(tmp>>8);
if (sequence[31] < MAX_CHANNELS)
tmp = read_analog(31);
else
tmp = 0;
out_data[3] = (int8)(tmp);
out_data[4] = (int8)(tmp>>8);
while (!can_tbe()) ;
tx_id = ID_BASE;
tx_id |= ((_board_ID) << 4);
//tx_id |= ((0 & 0x00f0) >> 4);
out_data[0] = 0x30+30;
tx_len = 5;
can_putd(tx_id, out_data, tx_len, tx_pri, tx_ext, tx_rtr);
}
if (can_getd(rx_id, &in_data[0], rx_len, rxstat))
{
// handles message for the analog channel
if ((rx_len == 1) && ((rx_id & 0x700) == 0x200) && (in_data[0] < MAX_CHANNELS))
{
tmp = read_analog(in_data[0]);
out_data[1] = (int8)(tmp);
out_data[2] = (int8)(tmp>>8);
while (!can_tbe()) ;
tx_id = ID_BASE;
tx_id |= ((_board_ID) << 4);
tx_id |= ((rx_id & 0x00f0) >> 4);
out_data[0] = in_data[0];
tx_len = 3;
// replies to message.
can_putd(tx_id, out_data, tx_len, tx_pri, tx_ext, tx_rtr);
}
// handles message to prepare a sequence (32).
else
if ((rx_len == 5) && ((rx_id & 0x700) == 0x200) && (in_data[0] == MAX_CHANNELS))
{
for (i = 0; i < MAX_CHANNELS; i++)
sequence[i] = MAX_CHANNELS;
// perhaps this is not needed.
while (!can_tbe()) ;
tx_id = ID_BASE;
tx_id |= ((_board_ID) << 4);
tx_id |= ((rx_id & 0x00f0) >> 4);
out_data[0] = in_data[0];
tx_len = 1;
setup_timer_1(T1_DISABLED);
// replies to message.
can_putd(tx_id, out_data, tx_len, tx_pri, tx_ext, tx_rtr);
}
// handles message to prepare a broadcast sequence (33).
else
if ((rx_len == 5) && ((rx_id & 0x700) == 0x200) && (in_data[0] == MAX_CHANNELS+1))
{
for (i = 0; i < MAX_CHANNELS; i++)
{
val = *((int32 *)(&in_data[1]));
if (val & 0x00000001)
{
sequence[MAX_CHANNELS-1-i] = MAX_CHANNELS-1-i;
}
else
{
sequence[MAX_CHANNELS-1-i] = MAX_CHANNELS;
}
val >>= 1;
}
// perhaps this is not needed.
// LATER: check the driver.
while (!can_tbe()) ;
tx_id = ID_BASE;
tx_id |= ((_board_ID) << 4);
tx_id |= ((rx_id & 0x00f0) >> 4);
out_data[0] = in_data[0];
tx_len = 1;
val = *((int32 *)(&in_data[1]));
if (val == 0)
{
setup_timer_1(T1_DISABLED);
}
else
{
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);
set_timer1(65536-5000);
}
// replies to message.
can_putd(tx_id, out_data, tx_len, tx_pri, tx_ext, tx_rtr);
}
// handles CAN bus messages for the downloader
else
if ((rx_len == 1) && (((rx_id>>8) & 0x7)==7))
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.02;
float kd = 0.04;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
int leftSensor, rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250; //Sensor Threshold
int totalWidth;
//Primary Initialization
init(1);
//Networking Section
//Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 60, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 180, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 5){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, 50);
Sleep(0, 100000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
// set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
set_motor(2, -((int) (((whiteLocation*40/120)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
}
return 0;
}
int main(void)
{
uint8_t sensor; // Stores analog readings for transmission
int i; // multipurpose counter
char choice = 0; // Holds the top-level menu character
// int red = 0; // For the red LED intensity
// int green = 0; // For the green LED intensity
// int blue = 0; // For the blue LED intensity
uint8_t exit = 0; // Flag that gets set if we need to go back to idle mode.
unsigned int temph=0; // Temporary variable (typically stores high byte of a 16-bit int )
unsigned int templ=0; // Temporary variable (typically stores low byte of a 16-bit int)
uint8_t location = 0; // Holds the EEPROM location of a stored IR signal
// unsigned int frequency = 0; // For PWM control - PWM frequency, also used to set buzzer frequency
// int amplitude;
int channel;
unsigned int duty; // For PWM control - PWM duty cycle
int baud; // Baud rate selection for auxiliary UART
char scale; // For auxiliary UART
long int time_out=0; // Counter which counts to a preset level corresponding to roughly 1 minute
// Initialize system
// Turn off JTAG to save a bit of battery life
CCP = CCP_IOREG_gc;
MCU.MCUCR = 1;
init_clock();
init_led();
init_adc();
init_ir();
init_BT();
init_dac();
init_buzzer();
initAccel();
init_aux_uart(131, -3); // Set the auxiliary uart to 115200 8n1
EEPROM_DisableMapping();
// Enable global interrupts
sei();
// Do the following indefinitely
while(1) {
// Turn off LED
set_led(0,0,0);
// Reset flags (in case this isn't the first time through the loop)
exit = 0;
choice = 0;
time_out = 0;
stop_ir_timer();
// Sing a BL song in idle mode so you can be found. Stop as soon as you get a *
while(choice != 42) {
bt_putchar('B');
bt_putchar('L');
if (USART_RXBufferData_Available(&BT_data)) {
choice = USART_RXBuffer_GetByte(&BT_data);
if (choice == 128) {
// Something is trying to connect directly to an iRobot
set_aux_baud_rate( ROOMBA_UART_SETTINGS ); // depends on model
aux_putchar( 128); // pass through to iRobot
serial_bridge(); // currently never returns
}
else if (choice == 0) {
// Something may be trying to connect directly to a MindFlex headset
set_aux_baud_rate( 135, -2); // 57600
aux_putchar( 0); // pass through to headset
serial_bridge(); // currently never returns;
}
else {
bt_putchar(choice);
}
}
if (choice != 42)
_delay_ms(500);
}
// Active part of the program - listens for commands and responds as necessary
while(exit == 0) {
// Checks if we haven't heard anything for a long time, in which case we exit loop and go back to idle mode
time_out++;
// Corresponds to roughly 60 seconds
if(time_out > 33840000) {
exit = 1;
}
// Check for a command character
if (USART_RXBufferData_Available(&BT_data)) {
choice = USART_RXBuffer_GetByte(&BT_data);
}
else {
choice = 0;
}
// If it exists, act on command
if(choice != 0) {
// Reset the time out
time_out = 0;
// Return the command so the host knows we got it
bt_putchar(choice);
// Giant switch statement to decide what to do with the command
switch(choice) {
// Return the currect accelerometer data - X, Y, Z, and status (contains tapped and shaken bits)
case 'A':
updateAccel();
bt_putchar(_acc.x);
bt_putchar(_acc.y);
bt_putchar(_acc.z);
bt_putchar(_acc.status);
break;
// Set the buzzer
case 'B': // frequency_divider(2)
if (get_arguments(2)) {
set_buzzer(GET_16BIT_ARGUMENT(0));
}
break;
// Turn off the buzzer
case 'b':
turn_off_buzzer();
break;
// Returns the value of the light sensor
case 'L':
sensor = read_analog(LIGHT, 0);
bt_putchar(sensor);
break;
// Returns the Xmegas internal temperature read - this is undocumented because the value returned is very erratic
case 'T':
sensor = read_internal_temperature();
bt_putchar(sensor);
break;
// Returns the battery voltage
case 'V':
sensor = read_analog(BATT_VOLT, 0);
bt_putchar(sensor);
break;
// Differential ADC mode
case 'D': // active(1) numgainstages(1)
if (get_arguments(2)) {
if (arguments[0] == '0') {
adcResolution = ADC_RESOLUTION_8BIT_gc;
adcConvMode = ADC_ConvMode_Unsigned;
adcGain = ADC_DRIVER_CH_GAIN_NONE;
adcInputMode = ADC_CH_INPUTMODE_SINGLEENDED_gc;
init_adc();
}
else if (arguments[0] == '1') {
adcResolution = ADC_RESOLUTION_12BIT_gc;
adcConvMode = ADC_ConvMode_Signed;
if (arguments[1] >= '1' && arguments[1] <= '6') { // number of gain stages
adcGain = (arguments[1] - '0') << ADC_CH_GAINFAC_gp;
adcInputMode = ADC_CH_INPUTMODE_DIFFWGAIN_gc;
init_adc();
}
else if (arguments[1] == '0') {
adcGain = ADC_DRIVER_CH_GAIN_NONE;
adcInputMode = ADC_CH_INPUTMODE_DIFF_gc;
init_adc();
}
else {
err();
}
}
else {
err();
}
}
break;
// Returns the readings on all six ADC ports
case 'X':
for (i=0; i<6; i++) {
sensor = read_analog(pgm_read_byte_near(muxPosPins+i), 0);
bt_putchar(sensor);
}
break;
// Returns differential measurement on pair of ADC ports
// Assumes we are in differential mode
case 'x':
if (get_arguments(2)) {
i = read_differential(arguments[0], arguments[1]);
bt_putchar(0xFF&(i >> 8));
bt_putchar(0xFF&i);
}
break;
case 'e': // this is a bit of testing code, which will eventually be changed
// do not rely on it
if (get_arguments(2)) {
char a1 = arguments[0];
char a2 = arguments[1];
while(1) {
_delay_ms(2);
i = read_differential(a1, a2);
itoa((i<<4)>>4, (char*)arguments, 10);
for (i=0; arguments[i]; i++)
bt_putchar(arguments[i]);
bt_putchar('\r');
bt_putchar('\n');
}
}
break;
// Sets the full-color LED
case 'O': // red(1) green(1) blue(1);
if (get_arguments(3)) {
set_led(arguments[0], arguments[1], arguments[2]);
}
break;
// Switches serial between irDA and standard serial
// Currently, this works over the same port numbers as
// standard serial, instead of using the IR receiver
// and IR LED. This may change when I get the IR receiver
// and LED working reliably.
case 'J':
temph = bt_getchar_timeout_echo();
if(/*temph == 256 || */ temph < '0' || temph > '1') {
err();
break;
}
set_irda_mode(temph - '0');
break;
// Sets up the IR transmitter with signal characteristics
case 'I':
init_ir();
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the frequency of the IR carrier
robotData.frequency = ((temph)<<8) + templ;
set_ir_carrier(robotData.frequency);
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
else {
// Set the length of the start up pulses
robotData.startUpPulseLength = templ;
}
if(robotData.startUpPulseLength > 16) {
err();
break;
}
// Read in the start up pulse timing data
for(i=0; i < robotData.startUpPulseLength; i++) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.startUpPulse[i] = ((temph)<<8) + templ;
}
if(temph == 256 || templ == 256) {
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the bit encoding to one of four pre-determined settings (see protocol instructions for more information)
robotData.bitEncoding = templ;
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the number of bits (and bytes) contained in an IR command
robotData.numBits = templ;
robotData.numBytes = (robotData.numBits-1)/8 + 1;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set timing data for a high bit
robotData.highBitTime = ((temph)<<8) + templ;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Set timing data for a low bit
robotData.lowBitTime = ((temph)<<8) + templ;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set timing data for on or off
robotData.offTime = ((temph)<<8) + templ;
break;
// Transmit an IR signal according to the previously determined configuration
case 'i':
init_ir();
// Get the signal data as one or more bytes
for(i = 0; i < robotData.numBytes; i++) {
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.irBytes[i] = templ;
}
if(templ == 256) {
break;
}
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Determine if the signal is repeated or not, and if so, with what frequency
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
// Startup timer interrupts
start_ir_timer();
break;
// Turn off any repeating IR signal
case '!':
robotData.repeatFlag = 0;
stop_ir_timer();
break;
// Capture a signal from the IR receiver
case 'R':
init_ir_read();
while(ir_read_flag!=0);
break;
// Store the captured signal in an EEPROM location
case 'S':
location = bt_getchar_timeout_echo()-48; // Subtracting 48 converts from ASCII to numeric numbers
if((location >= 0) && (location < 5) && (signal_count > 4)) {
write_data_to_eeprom(location);
}
else {
err();
}
break;
// Receive a raw IR signal over bluetooth and transmit it with the IR LED
case 's':
if(read_data_from_serial()) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set if the signal should repeat and if so, with what frequency
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
// Set frequency to 38KHz, since raw signals must have come from the receiver at some point
robotData.frequency = 0x0349;
robotData.startUpPulseLength = 0;
robotData.bitEncoding = 0x04;
start_ir_timer();
}
else {
err();
break;
}
break;
// Get a stored signal from an EEPROM location and transmit it over the IR LED (and repeat as desired)
case 'G':
location = bt_getchar_timeout_echo()-48;
if(location >= 0 && location < 5) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
read_data_from_eeprom(location);
robotData.frequency = 0x0349;
robotData.startUpPulseLength = 0;
robotData.bitEncoding = 0x04;
start_ir_timer();
}
else {
err();
}
break;
// Get a stored signal from EEPROM and print it over bluetooth to the host
case 'g':
location = bt_getchar_timeout_echo()-48;
if(location >= 0 && location < 5) {
print_data_from_eeprom(location);
}
else {
err();
}
break;
// Output on digital I/O
case '>':
// Set port
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Get value
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
set_output(temph, (templ-48));
break;
// Input on digital I/O
case '<':
// Get port
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Get value (1 or 0)
templ = read_input(temph)+48;
bt_putchar(templ);
break;
// Configure PWM frequency
case 'P':
if (get_arguments(2))
pwm_frequency = GET_16BIT_ARGUMENT(0);
break;
// Set PWM duty cycle for a specific port
case 'p':
if (get_arguments(3)) {
set_pwm();
duty = GET_16BIT_ARGUMENT(1);
if (arguments[0] == '0')
set_pwm0(duty);
else if (arguments[1] == '1')
set_pwm1(duty);
// could add else err();, but original firmware doesn't do this
}
break;
// Set DAC voltage on one of the two DAC ports
case 'd':
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
if(temph == '0') {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
else {
set_dac0(temph);
}
}
else if(temph == '1') {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
else {
set_dac1(temph);
}
}
break;
// Go back to idle mode so you can be found again. Should be sent at end of host program.
case 'Q':
exit = 1;
break;
// Human-readable uart speed setting
case 'u':
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
baud = ((temph)<<8) + templ;
switch(baud) {
case ('1'<<8) + '2': // 1200
baud = 6663;
scale = -2;
break;
case ('4'<<8) + '8': // 4800
baud = 3325;
scale = -3;
break;
case ('9'<<8) + '6': // 9600
baud = 829;
scale = -2;
break;
case ('1'<<8) + '9': // 19200
baud = 825;
scale = -3;
break;
case ('3'<<8) + '8': // 38400
baud = 204;
scale = -2;
break;
case ('5'<<8) + '7': // 57600
baud = 135;
scale = -2;
break;
case ('1'<<8) + '1': // 115200
baud = 131;
scale = -3;
break;
default:
err();
goto BAUD_DONE;
}
set_aux_baud_rate(baud, scale);
BAUD_DONE:
break;
// Configures the baud rate of the auxiliary UART
case 'C': // baud(2) scale(1)
// e.g., 9600 = C\x03\x3D\xFE
if (! get_arguments(3))
break;
bt_putchar(arguments[2]); // duplicate buggy behavior from official firmware; delete if unwanted
set_aux_baud_rate( GET_16BIT_ARGUMENT(0), arguments[2]);
break;
// BT-serial high speed bridge mode
case 'Z':
serial_bridge();
break;
case 'w': // channel(1) type(1) duty(1) amplitude(1) frequency(3)
// w0s\x20\xFF\x02\x00\x00
if (!get_arguments(7))
break;
if (arguments[0] < '0' || arguments[0] > '1' || arguments[2] > WAVEFORM_SIZE) {
err();
break;
}
play_wave_dac(arguments[0]-'0', (char)arguments[1], arguments[2], arguments[3], get_24bit_argument(4));
break;
case 'W': // channel(1) frequency(3) length(1) data(length)
if (!get_arguments(5))
break;
if (arguments[0] < '0' || arguments[0] > '1' || arguments[4] > WAVEFORM_SIZE || arguments[4] == 0 ) {
err();
break;
}
channel = arguments[0] - '0';
if (bt_to_buffer(waveform[channel], arguments[4])) {
disable_waveform(channel);
play_arb_wave(channel, waveform[channel], arguments[4], get_24bit_argument(1));
}
break;
case '@':
channel = bt_getchar_timeout_echo();
if (channel == '0')
disable_waveform0();
else if (channel == '1')
disable_waveform1();
else
err();
break;
case 'r':
i = USART_RXBufferData_AvailableCount(&AUX_data);
bt_putchar((uint8_t)(i+1));
while(i-- > 0) {
bt_putchar(USART_RXBuffer_GetByte(&AUX_data));
}
break;
// Transmit a stream of characters from bluetooth to auxiliary serial
case 't':
temph= bt_getchar_timeout_echo();
if (temph == 256) {
err();
break;
}
for(; temph>0; temph--) {
templ= bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
else {
aux_putchar( templ);
}
}
break;
default:
break;
}
}
int main()
{
//Open gate:
//connects to server
//connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
//send_to_server("Please");
//send_to_server("123456");
//receives message from the connected server
//char message[24];
//receive_from_server(message); //this line looks buggy, is it right?
//send_to_server(message);
int i = 0; // Re-added this because didnt seem to complie without it
int baseSpeed = 35;
float kp = 0.03; //random constant
float kd = 0;
init(0);
// connect camera to the screen
open_screen_stream();
// set all didgital outputs to +5V
for (int i = 0; i < 8; i++)
{
// set all digital channels as outputs
select_IO(i,0);
write_digital(i,1);
}
while(1)
{
take_picture(); // take camera shot
int white[320];
int value;
// we made the array 320 because that is the width of pixels
// draw some line
for(int i = 0; i < 320; i++){
set_pixel(i, 55 ,255,0,0);//redline
value = get_pixel(i,56,3); // give each pixel in the array the pixel value of its location based on ' i '.
if(value > 100){ // change 70 to actual white line value later
white[i] = 1;
}
else{
white[i] = 0;
}
//printf("%d\n",white[i]); // print array results
}
//process the data collected so far:
//but first create an array counting from -170 to 170
int e=0;
int sum = 0;
for(int i=0;i<320;i++){
sum = sum + (i - 160)*white[i];
}
int previous_error; //-We removed the initialisation of this to 0 because it would be set to 0 every time it looped.
// think as it is now, the previous error will always be zero when the wile loop goes round.
int current_error = 0;
int derivative_signal;
int error = 0;
for(int i=0;i<320;i++){ // This looks very similar to what we already have above to to the proportional error
error = (i - 160)*white[i]; // Maybe we can combine these in some way?
current_error += error;
}
//Sleep(0,100000); // This could be a problem - removed because we already have a sleep command
derivative_signal = (current_error-previous_error/0.1)*kd; //I think this si the equivalent of 'e = (sum/20)*kp;' below
previous_error = current_error;
printf("Derivative signal is: %d", derivative_signal );
e = sum*kp;
int LM = baseSpeed+e+derivative_signal;
int RM = baseSpeed+(-1*e)+(-1*derivative_signal);
printf("%d\n",LM);
set_motor(1, LM);
set_motor(2, RM);
// display picture
update_screen();
Sleep(0,100000);
for (i = 0 ; i < 8; i++)
{
int av = read_analog(i);
// printf("ai=%d av=%d\n",i,av);
}
}
// terminate hardware
close_screen_stream();
set_motor(1,0);
set_motor(2,0);
return 0;
}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.0;
float kd = 0.0;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
//int motorOne, motorTwo;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
signed int leftSensor;
signed int rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250;
int totalWidth;
signed int leftMotor, rightMotor;
//Primary Initialization
init(1);
//Networking Section
// Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
set_motor(1, 43);
set_motor(2, -40);
Sleep (5,0);
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 10, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 230, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 3){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 300000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// motorOne = (-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))
// motorTwo = (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)
// set_motor(1, motorOne);
// set_motor(2, -motorTwo);
//set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
//set_motor(2, -((int) (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
whiteWall = 0;
//returns true if there isnt a wall
noLeftWall =false;
noRightWall = false;
noWallAhead = false;
//get data from sensors
leftSensor = read_analog(0);
rightSensor = read_analog(1);
//printf("left sensor: %d\nright sensor: %d\n", leftSensor, rightSensor);
//get data from camera
take_picture();
for(int i = 120; i<128; i++){
c = get_pixel(300,i, 3);
if(c>120){ //change white threshold
whiteWall++;
}
}
printf("whiteWall: %d\n", whiteWall);
if(whiteWall < 5){ //Change threshold if theres problems
noWallAhead = true; //rename
// printf("No wall ahead!!!!!!!!!\n\n\n");
}
if(leftSensor<THRESHOLD){
noLeftWall = true;
}
if(rightSensor<THRESHOLD){
noRightWall = true;
}
if(noRightWall){
set_motor(1, 32);
set_motor(2, -30);
Sleep(0, 300000);
set_motor(1, 37);//right motor
set_motor(2, -67);//left motor//CHANGE THRESHOLD
Sleep(0,900000);//CHANGE THRESHOLD
printf("turning right\n");
}
else if(noWallAhead){ //stay in the center of the maze
rightMotor = (leftSensor/10*1.1); //change threshold
leftMotor = -(rightSensor/10);
set_motor(1, rightMotor);
set_motor(2,leftMotor);
Sleep(0,1);
//rotate back to centre
leftMotor = -(leftSensor/10); //change threshold
rightMotor = (rightSensor/10*1.1);
set_motor(1, rightMotor);
set_motor(2, leftMotor);
printf("forwarsdgsdjksdgr\n");
Sleep(0, 1);
}
else if(noLeftWall){
set_motor(1, 40);
set_motor(2,-42);
Sleep(0,450000);
set_motor(1, 66);//right motor
set_motor(2, -32);//left motor //Change thresholds
Sleep(0,900000);
printf("left left left left\n");
}
// else //pop a u turn
/* {
printf("pop a u turn\n");
set_motor(1, -50);
set_motor(2, -60); //bigger so the back doesn't hit the wall
Sleep(0,100000); //Change thresholds
}*/
}
return 0;
}
int main()
{
init(0);
signal(SIGINT, terminate);
signal(SIGTERM, terminate);
if (gate_test == 1) {
//connects to server with the ip address 130.195.6.196
connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
send_to_server("Please"); // "Please is the 'codeword', may change
//receives message from the connected server
char message[24];
receive_from_server(message);
send_to_server(message); // send password back to server to open up gate.
}
Sleep(1, 000000);
speed = 90;
v_left = speed - Tcorrection; // speed of left motor
v_right = speed + Tcorrection; // speed of right motor
// Method for completing quadrant 1
while (method == 1) {
Dconstant = 340;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
midPic[i] = get_pixel(x, 1, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + (zeroCentre[l] * midPic[l]);
}
if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == NTP) {
method = 2;
}
else if (n_whites_mid == 0) {
// if loses line, swings quickly to direction of last error of line seen
if (Perror_mid > 0) {
// hard left
v_left = -15;
v_right = 40;
}
else if (Perror_mid < 0) {
// hard right
v_left = 40;
v_right = -15;
}
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
// method for completing quadrant 3
while (method == 2) {
speed = 65;
Pconstant = 0.6;
Dconstant = 375;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
n_whites_left = 0;
n_whites_right = 0;
sensor_right = read_analog(2);
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
int y = dy * i + (dy / 2);
midPic[i] = get_pixel(x, 1, 3);
leftPic[i] = get_pixel(1, y, 3);
rightPic[i] = get_pixel(319, y, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
if (leftPic[i] > THRESHOLD) {
// 1 = white
leftPic[i] = 1;
n_whites_left++;
}
else {
// 0 = black
leftPic[i] = 0;
}
if (rightPic[i] > THRESHOLD) {
// 1 = white
rightPic[i] = 1;
n_whites_right++;
}
else {
// 0 = black
rightPic[i] = 0;
}
if (sensor_right > 300) {
method = 3;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + zeroCentre[l] * midPic[l];
}
// t junction
if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process(); // calls method to take image and process values
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
// left turn
else if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right < 5) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
//right turn
else if (midPic[16] == 0 && n_whites_left < 5 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// dead end - should do 180
else if (n_whites_mid == 0 && n_whites_left == 0 && n_whites_right == 0) {
while (midPic[16] == 0) {
Quad3Process();
v_left = -speed;
v_right = speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// pid
else if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == 0) {
v_left = 45;
v_right = 45;
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
while (method == 3) {
Pconstant = 0.6;
int error;
Perror_mid = error;
speed = 40;
Dconstant = 0.35;
// Loop here:
sensor_left = read_analog(0);
sensor_mid = read_analog(1);
sensor_right = read_analog(2);
error = sensor_right - sensor_left;
proportional = error * Pconstant;
proportional = ((proportional / 770) * 100);
derivative = ((error - Perror_mid) / 0.1) * Dconstant;
if (sensor_mid > 470) {
set_motor(1, -speed);
set_motor(2, speed);
Sleep(0, 350000);
}
set_motor(2, speed - proportional);
set_motor(1, speed + proportional);
}
}
int main (){
init_hardware();
// init(0);
int brightnessArray[WIDTH];
int threshold = 120;
int generatedArray[WIDTH];
int errorArray[WIDTH];
int lagErrorArray[WIDTH];
int baseSpeed = 35;
int leftSpeed=baseSpeed;
int rightSpeed=baseSpeed;
bool check=true;
int countrev=0;
int previousError = 0;
int derivative = 0.0;
double kd = 0.00000005;
bool checkintersection=false;
int intcount=0;
int sumError=0;
int incount=0;
bool right90=true;
bool left90=true;
int stuckCount = 0;
//network
bool gate = true;
char ip[14] = "130.195.6.196";
char message[7];
int ir_sensor = read_analog(0);
//connect_to_server(ip,1024);
double k=0.0008; // constant
// an array starting from -160 to 160
int number = -160;
for(int y=0; y<WIDTH+1; y++){
generatedArray[y] = number;
number++;
}
int counter = 0;
int zeroCount = 0;
while(true){
take_picture();
ir_sensor=read_analog(0);
//display_picture(0.5,0);
//open_screen_stream();
//update_screen();
previousError = sumError;
sumError=0;
for(int i=0; i<WIDTH; i++){
char colour = get_pixel(i, HEIGHT/2, 3);
if(colour > threshold){
brightnessArray[i] = 1;
} else {
brightnessArray[i] = 0;
}
errorArray[i] = brightnessArray[i] * generatedArray[i];
sumError += errorArray[i];
}
derivative = ((sumError-previousError)/0.000025);//.000025
printf("Sum error: %d\n",sumError);
printf("Previous error: %d\n",previousError);
//90 DEGREE TURNS
//Right and left 90 Degree turn
//int nine=0;
/*
int turnCounter = 0;
if(sumError>10000){ //sum should be large as the right hand side(0-160) will be white
right90=true;
}else{
right90=false;
}
if(sumError<-10000){ //sum should be very small as the left hand side(0 to -160) will be white
left90=true;
}else{
left90=false;
}
if(right90){
while(turnCounter<25){
set_motor(0,baseSpeed);
set_motor(1,-baseSpeed);
turnCounter++;
}
}
if(left90){
while(sumError<25){
set_motor(0,-baseSpeed);
set_motor(1,baseSpeed);
turnCounter++;
}
}
turnCounter=0;
*/
// calculate speed for left and right motors k has to be small like 0.1
//INTERSECTION CODE
//for(int in=0;in<WIDTh;in++){
//if((birghtnessArray[h])==1){ //Goes through array and because the camera should read all white the sum should have very few 0's
//checkintersection=true; //if it counts 4 blacks then its not an intersection
//}
//else if{intcount<4){
//checkintersection=true;
//incount++
//}
//else if(intcount==4){
//checkintersection=false;
//break;
//}
//}
//if(checkintersection){
//set_motor(0,45)
//set_motor(1,45)
//}
//}//~~~~~unsure what this is for/what it closes off
for(int h=0;h<WIDTH;h++){
if((brightnessArray[h])==0){
check=false;
}else if(countrev<4){
check=false;//this used to be true, changed it to false in case of the last error vlaue.
countrev++;
}else if (countrev==4){
check=true;
break;
}/*
if ((brightnessArray[h])==1){
checkintersection=true;
}else if(intcount<4){
checkintersection=true;
incount++;
}else if(intcount==4){
checkintersection=false;
break;
}*/
//continue;
}
/*for(int i=0; i<WIDTH;i++){
if(brightnessArray[i]==1){
checkintersection=true;
}else if(intcount<1){
checkintersection = true;
incount++;
}else if(intcount==1){
checkintersection=false;
break;
}
}*/
if(check==true){ // && checkintersection==false){
// printf("Kd*d: %i\n",(kd*derivative));
leftSpeed = baseSpeed + (int)(((k*sumError)-(kd*derivative))); // sumError should be 0 for the robot to go in a straight line
rightSpeed = baseSpeed - (int)(((k*sumError)+(kd*derivative)));
printf("K*sum: %d\n",(k*sumError));
printf("Derivative: %d\n",derivative);
/* if(leftSpeed<baseSpeed){
leftSpeed = 0
} else if(rightSpeed<baseSpeed){
rightSpeed = 0;
}
*/
if(leftSpeed > 255){
leftSpeed = 255;
}
else if(leftSpeed < -255){
leftSpeed = -255;
}
if(rightSpeed > 255){
rightSpeed = 255;
} else if(leftSpeed < -255){
rightSpeed = -255;
}/*
if(sumError>6000 || sumError<-6000){
set_motor(1, baseSpeed);
set_motor(2, baseSpeed);
}*/
if(sumError==0 && previousError<0){
leftSpeed = 0;
rightSpeed = baseSpeed;
}
else if(sumError==0 && previousError>0){
leftSpeed = baseSpeed;
rightSpeed = 0;
}/*else if(sumError==0 && previousError==0){
set_motor(1, baseSpeed);
set_motor(2,-baseSpeed);
}*/
else {
set_motor(1, leftSpeed); // Motor 1 is left motor//left
set_motor(2, rightSpeed);//right
}
printf("Left Speed = %d, Right Speed =%d\n", leftSpeed, rightSpeed);
printf("Sum error: %d\n",sumError);
Sleep(0, 25000); // sleeps for 50 ms
counter++;
if(counter==1000){
set_motor(1, leftSpeed); // Motor 1 is left motor
set_motor(2, rightSpeed);
}
} else if(check==false){// && checkintersection==false) {
//int countback=0;
//while(countback<4){
set_motor(1,-baseSpeed);
set_motor(2,-baseSpeed);
//Sleep(0,5000000);
//countback++;
//}
}/* else if(check==true){
set_motor(1,baseSpeed);
set_motor(2,baseSpeed);
Sleep(3,0);
set_motor(1,0);
set_motor(2, baseSpeed);
}*/
if(gate==true && ir_sensor>200){
// printf("%d", ir_sensor);
set_motor(1, 0);
set_motor(2, 0);
connect_to_server(ip, 1024);
//if(ir_sensor > 1){
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//gate = true;
//}
gate=false;
Sleep(2,0);
//might be buggy and need to double check
//printf("%s", message);
}
}
return 0;
}
void testSensors(){
//int select_IO(0, 1);
int analog_sensor_reading = read_analog(0);
printf("%d\n",analog_sensor_reading);
Sleep(0,500000);
}
int main(){
//This sets up the RPi hardware and ensures everything is working properly
init(0);
//connect_to_server("130.195.6.196", 1024);
//sends message
//send_to_server("Please");
//receives message
//char message[24];
//receive_from_server(message);
//send_to_server(message);
select_IO(2, 1); //right sensor
select_IO(4, 1); //left sensor
//right wheel
set_motor(1, minSpeed);
// left wheel
set_motor(2, minSpeed);
//infinite loop for testing purposes
while(true){
//Take picture with camera
take_picture();
prevError = error;
error=0;
errorR=0;
errorL=0;
sum=0;
red=0;
for (int i=0; i<160; i++){
w = get_pixel(i, 120, 3);
r=get_pixel(i,120,0);
if(r>210){
red++;
}
if(w > 120){
errorL++;
}
}
for (int i=160; i<320; i++){
//this is calculating the sum for the whole width of pixels as opposed to just right, there is no range?
// possibly the condition for the loop should be
//for (int i=0; i<320 && i>160; i++){
w = get_pixel(i, 120, 3);
r=get_pixel(i,120,0);
if(r>210){
red++;
}
if(w > 120){
errorR++;
}
}
error=errorR-errorL;
//rests for 0.1 seconds
Sleep(0,1000);
sum=errorR+errorL;
//when it reaches the secount quadrant
if((sum)>310){
//this could be made true when doing the first set of curves, causing it to break into the
First=0;
//intersection code
minSpeed=70;
}
int test=read_analog(2);
int test1=read_analog(4);
if(First==0&&test>400&&test1>400){
First=2;
minSpeed = 60;
}
if(First==2){
prevError=0;
break;
}
if((sum>10)&&First==1){
//Proportional Signal
propSignal = (error)*Kp;
propSignal=(propSignal/160)*200;
//Integral Signal
sumError += error;
intSignal = (sumError)*Ki;
//trial and error find an x value that works
derSignal = ((error-prevError)/sleepTime)*Kd;
//right wheel
set_motor(1, minSpeed + (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed - (propSignal + intSignal + derSignal));
}else if(First==1){ //Do we need this, this could be causing the initial spin
//turns until line is found.
set_motor(1, -50);
set_motor(2, -60);
//when it reaches the intersections
}else if((error>-20)&&(error<20)&&First==0&&sum>3){
//Proportional Signal
propSignal = (error)*Kp;
propSignal=(propSignal/160)*200;
//Integral Signal
sumError += error;
intSignal = (sumError)*Ki;
//trial and error find an x value that works
derSignal = ((error-prevError)/sleepTime)*Kd;
//right wheel
set_motor(1, minSpeed - (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed + (propSignal + intSignal + derSignal));
}else if(((errorL)>errorR+1)&&First==0){
//turns90 left
set_motor(2, 70);
set_motor(1, -40);
} else if(((errorL+1)<errorR)&&First==0){
//turns90 right
set_motor(2, -40);
set_motor(1, 70);
}else if(First==0){
//finds the line again
set_motor(1,-60);
set_motor(2, 60);
}
}
while(1){
double kp=0.6;
double kd=0.01;
int right=read_analog(2);
int left=read_analog(4);
error=right-left;
propSignal=error*kp;
propSignal=((propSignal/600)*100);
derSignal=(((error-prevError)/sleepTime)*kd);
prevError=error;
if (front>300){
set_motor(1,-50);
set_motor(2,70);
}else{
set_motor(1, minSpeed - (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed + (propSignal + intSignal + derSignal));
}
}
return 0;
}
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() {
init(0);
//Networking Code
connect_to_server("130.195.6.196",1024);
send_to_server("Please");
char message[24];
receive_from_server(message);
send_to_server(message);
//Signal Catcher code
signal(2, signal_callback_handler);
//Declaration of Constants for 2nd Quadrant
double errorSum = 0;
double errorSum2 = 0;
double kp = 0.5;
double kd = 5;
double proportional_signal;
open_screen_stream();
int quad_three = 0;
printf("QUADRANT ONE AND TWO\n");
while(1) {
quad_three = 0;
take_picture();
//CODE FOR PROPORTIONAL
for (int i=0; i < 320; i++){
if(get_pixel(i, 160, 3) > THRESH) {
errorSum += (i-160);
quad_three++;
}
}
//CODE FOR FUTURE LINE ERROR
for (int i=0; i < 320; i++){
if(get_pixel(i, 80, 3) > THRESH) {
errorSum2 += (i-160);
}
}
errorSum/=160;
errorSum2/=160;
//THIS IS NOT ACTUALLY DERIVATIVE SIGNAL - THIS CODE:
//Looks to see if there any curves in the line and slows down based on how curvy the curve is
int derivative_signal = abs(errorSum-errorSum2)*kd;
if(derivative_signal > 33) {
derivative_signal = 33;
}
proportional_signal = errorSum * kp;
int leftSpeed = BASE_SPEED + proportional_signal - derivative_signal;
int rightSpeed = BASE_SPEED - proportional_signal - derivative_signal;
if (proportional_signal < - 0.5 && proportional_signal > -0.6 && quad_three != 320){
for(int i = 0; i < 10; i++) {
move(BASE_BACK_SPEED, BASE_BACK_SPEED);
}
} else {
if(quad_three == 320)break;
move(leftSpeed, rightSpeed);
}
}
printf("THIRD QUADRANT\n");
move(50,50);
Sleep(0,200000);
while(1) {
if (p(160,0.3,50,0) > 310 && p(40,0.3,50,0) < 80) {
turn(1);
break;
}
}
int red = 0;
int blue = 0;
int green = 0;
while(red < 200 || green > 100 || blue > 100) {
take_picture();
red = get_pixel(120,160,0);
green = get_pixel(120,160,1);
blue = get_pixel(120,160,2);
int p160 = p(160,0.7,40,0);
int p80 = p(80,0.7,40,0);
if (p160 > 310 && p80 < 80) {
turn(1);
}
}
move(70,70);
Sleep(0,200000);
int same_count = 0;
double prev_signal = 0;
int count = 0;
int prevTotal = 0;
while (1){
int left_signal = read_analog(1);
int right_signal = read_analog(0);
int base_speed = 45;
double constant = 0.075;
/*printf("Left %d\n",left_signal);
printf("Right %d\n",right_signal);*/
double actual_signal = left_signal - right_signal;
actual_signal *= constant;
actual_signal += 4.8;
printf("Actual %f\n",actual_signal);
if (actual_signal > 20 || actual_signal < -20){
base_speed = 35;
} else {
base_speed = 45;
}
move(base_speed + actual_signal, (base_speed - actual_signal)*1.3);
}
}