/*robot.c neural network robot of craziness */ #include #include #include #include #include // Timeout Values for each task #define t1 2 //2 5 is debugging #define t4 20 //20 5000 is debugging #define t5 14000 //14000 10000 is debugging maybe 3000? #define t6 180000//180000 #define begin { #define end } // These defines effect values everywhere and the majority of the program will adjust when these are changes // The two sections that will not adjust is the membrane potential update and the sensor update, please be aware of this #define numneuron 8 #define numsensor 15 // Bits of Sensor #define numindivid 6 // These defines give range values for the sensors to indicate amount of spiking necessary (not is nothing, col is colission), temperature can really effect these values so they should be rechecked often #define sensor1not 4.658 #define sensor1top 4.556 #define sensor1mid 4.33 #define sensor1bot 4.14 #define sensor1col 3.403 #define sensor2not 4.551 #define sensor2top 4.531 #define sensor2mid 4.512 #define sensor2bot 4.492 #define sensor2col 4.243 #define sensor3not 4.79 #define sensor3top 4.64 #define sensor3mid 4.52 #define sensor3bot 4.219 #define sensor3col 3.300 #define sensor4not 4.219 #define sensor4top 4.141 #define sensor4mid 4.102 #define sensor4bot 3.965 #define sensor4col 3.5 #define sensor5not 4.375 #define sensor5top 4.336 #define sensor5mid 4.297 #define sensor5bot 4.238 #define sensor5col 3.535 // Thse defines are noise ranges for a random seed generation #define sensor6bot 1.30 #define sensor6top 1.36 void task1(void); // Neuron Implementation void task2(void); // Left Stepper Motor Control void task3(void); // Right Stepper Motor Control void task4(void); // Sensor/Motor Speed Update void task5(void); // Neuron Individual Selection and Mutation void task6(void); // Best Fitness Store void initialize(void); // Initialize // Task variables unsigned long time1, time2, time3, time4, time5, time6; // Task scheduling timeout counters unsigned int t2, t3; // Timeout Values for task 2 and 3, speed of motors will be determined by these values unsigned char sensorflag; // Flag that indicates Sensors have been updated (1 updated, 0 not updated) unsigned char evolveflag; // Flag that indicates a mutation has just occurred unsigned long tsk4cnt; // Counter for task 4 to allow us to update motors every 160 ms to allow for actual movement // Neuron Variables unsigned char individual; // Currently Selected Individual of the population (0 - 5) unsigned char neuron; // Output of Neurons (1 is spike, 0 is no spike), ie. 0b11001100 means neurons 2, 3, 6, 7 are spiking, neurons 0, 1, 4, 5 are not unsigned int sensor; // Output of Sensors (1 is spike, 0 is no spike), ie. 0b11001100 means sensors 2, 3, 6, 7 are spiking, sensors 0, 1, 4, 5 are not unsigned char signs[numindivid]; // Sign of Neurons (1 is positive, 0 is negative), ie. 0b00001111 means neurons 0, 1, 2, 3 are positive, neurons 4, 5, 6, 7 are not unsigned char nconnection[numindivid][numneuron]; // Connections from Neurons, each char describes connections for one neuron (1 is connection, 0 is no connection), ie. 0b11001010 means there are connections to this neuron from neurons 1, 3, 6, 7 and no connection from neurons 0, 2, 4, 5 unsigned int sconnection[numindivid][numneuron]; // Connections from Sensors, each char describes connections from one sensor (1 is connection, 0 is no connection) to each neuron signed int memb[numneuron]; // Membrane Potential of each Neuron (minimum value of 0, initialized to 0) float fitness[numindivid]; // Fitness value for an individual unsigned int threshold[numindivid]; // Membrane Threshold (5) - DEBUG BEING SET TO EVOLVE RANDOMLY BETWEEN 2 AND 6 unsigned char leaking; // Leaking Constant (1) unsigned char minimum; // Miminum Membrane Potential (0) unsigned int randomint; // Randomly generated integer // Old Variables unsigned char oldsigns; unsigned char oldnconnection[numneuron]; unsigned int oldsconnection[numneuron]; unsigned int oldthreshold; // ADC Variables unsigned int Ain; // Current ADC input unsigned int Ain2; // DEBUG for Noise for ADCL float voltage; // Current ADC input converted into voltage unsigned int sensorread; // Values of Sensors being read float probability; // Probability of getting a random noise spike float randomprob; // Random probability // Motor Variables unsigned char stepL; // Step variable for Left Motor unsigned char stepR; // Step variable for Right Motor unsigned char dirL; // Direction variable for Left Motor (0 Forward or 1 Backward) unsigned char dirR; // Direction variable for Right Motor (0 Forward or 1 Backward) unsigned char spikeFL; // Number of spikes during sensor update period (20 ms) generated by forward left neuron unsigned char totalspikeFL; // Total spikes between evolution unsigned char spikeBL; // Number of spikes during sensor update period (20 ms) generated by backward left neuron unsigned char totalspikeBL; // Total spikes between evolution unsigned char spikeFR; // Number of spikes during sensor update period (20 ms) generated by forward right neuron unsigned char totalspikeFR; // Total spikes between evolution unsigned char spikeBR; // Number of spikes during sensor update period (20 ms) generated by backward right neuron unsigned char totalspikeBR; // Total spikes between evolution unsigned long maxspikes; // Number of possible spikes during sensor update period float v2, v3; // Motor Velocities // Evolution Variables float newfitness; // Calculates new fitness to compare with old fitnesses unsigned char keep; // Keep determines if we will keep a mutation, is 1 if it the current individuals fitness is >= worst individuals fitness unsigned char collision; // Indicates a collision for fitness calculation (0 is no collision, 1 collision) float sensormaxactivity; // Most active sensor value for fitness calculation (5 values for this based on most active sensor: 0 if nothing, .25 if between top and mid, .5 if between mid and bot, .75 if between bot and col, 1 if below or equal to col) float sensoractivity[numsensor/3]; // Activity of 5 sensors based on values above to be maxed unsigned char worstindivid; // Worst indiviudal used to determine keep unsigned char randn, rands; // Random Variables to access blocks in random individual for mutation unsigned int randomseed; float change; // Change in fitness (debugging) // Fitness Store variables unsigned int bestfitness; // Best Fitness so far unsigned int bestindivid; // Best Individual // EEPROM STUFF eeprom unsigned char eeindex = 0; // Index of fitness values/replacements eeprom unsigned char eesigns[numindivid]; eeprom unsigned char eenconnection[numindivid][numneuron]; eeprom unsigned int eesconnection[numindivid][numneuron]; eeprom int eefitness[numindivid]; eeprom int eethreshold[numindivid]; eeprom int eebestfitnessmem[200]; eeprom unsigned char eebestsigns; eeprom unsigned char eebestnconnection[numneuron]; eeprom unsigned int eebestsconnection[numneuron]; eeprom int eebestfitness; eeprom int eebestthreshold; // For loop variables unsigned int i, j, k, x, a, b, c, d, e, f, y, z, g, h, ii, jj, kk, xx, yy, zz; //////////////////////////////////////////////////////////////// //timer 0 Compare Match ISR interrupt [TIM0_COMP] void timer0_overflow(void) begin if (time1>0) --time1; if (time2>0) --time2; if (time3>0) --time3; if (time4>0) --time4; if (time5>0) --time5; if (time6>0) --time6; if (tsk4cnt>0) --tsk4cnt; end //////////////////////////////////////////////////////////////// void task1(void) begin time1 = t1; // Sensor/Motor Updating if(sensorflag==0) sensor = 0; // If sensors aren't ready, sensor nodes shouldn't be spiking else begin sensor = sensorread; sensorflag = 0; spikeFL = 0; spikeBL = 0; spikeFR = 0; spikeBR = 0; maxspikes = 0; end if(evolveflag==1) // If just evolved begin // Spike Forward Motor Neurons to start memb[0] = threshold[individual] + 20; memb[2] = threshold[individual] + 20; // Debugging //memb[4] = threshold[individual] + 20; //memb[5] = threshold[individual] + 20; //memb[6] = threshold[individual] + 20; //memb[7] = threshold[individual] + 20; evolveflag = 0; totalspikeFL = 0; totalspikeBL = 0; totalspikeFR = 0; totalspikeBR = 0; end // Debugging //printf("%lx\n\r", neuron); //////////////Refractory Period/Contribution of Incoming Spikes/Membrane Potential Update////////////// for(i = 0; i < numneuron; i++) begin if(((neuron>>i)&0x01)==0) // Refractory Period Check (if spike generated in previous update, don't update membrane potential) begin // Calculate Contribution of Sensors and update membrane potential memb[i] = (memb[i] + (((sensor & sconnection[individual][i]) & 0b10000000)>>7) + (((sensor & sconnection[individual][i]) & 0b01000000)>>6) + (((sensor & sconnection[individual][i]) & 0b00100000)>>5) + (((sensor & sconnection[individual][i]) & 0b00010000)>>4) + (((sensor & sconnection[individual][i]) & 0b00001000)>>3) + (((sensor & sconnection[individual][i]) & 0b00000100)>>2) + (((sensor & sconnection[individual][i]) & 0b00000010)>>1) + (((sensor & sconnection[individual][i]) & 0b00000001)>>0)); // Calculate Contribution of Positive Neurons and update membrane potential memb[i] = (memb[i] + (((neuron & signs[individual] & nconnection[individual][i]) & 0b10000000)>>7) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b01000000)>>6) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00100000)>>5) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00010000)>>4) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00001000)>>3) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00000100)>>2) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00000010)>>1) + (((neuron & signs[individual] & nconnection[individual][i]) & 0b00000001)>>0)); // Calculate Contribution of Negative Neurons and update membrane potential memb[i] = (memb[i] - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b10000000)>>7) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b01000000)>>6) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00100000)>>5) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00010000)>>4) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00001000)>>3) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00000100)>>2) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00000010)>>1) - (((neuron & ((~signs[individual]) & 0xFF) & nconnection[individual][i]) & 0b00000001)>>0)); // Minumimum of memb is 0 if(memb[i] < minimum) begin memb[i] = minimum; end end // Ends if statement for Neurons not in Refractory Period end // Ends for loop for going through Neurons to update membrane potential ///////////////////////////////////////////Spike Generation//////////////////////////////////////////// for(j = 0; j < numneuron; j++) begin randomint = rand(); // rand generates 15 bits (need to generate random number between 2 and -2, 0 - 6553 is -2, 6554 - 13107 is -1, 13108 - 19660 is 0, 19661 - 26214 is 1, 26215 - 32767 is 2 if(randomint >= 0 && randomint <= 6553) // 6554 begin if(memb[j]>= (threshold[individual] - 2)) begin neuron = (neuron|(1<= 6554 && randomint <= 13107) // 6554 begin if(memb[j]>= (threshold[individual] - 1)) begin neuron = (neuron|(1<= 13108 && randomint <= 19660) // 6553 begin if(memb[j]>= (threshold[individual])) begin neuron = (neuron|(1<= 19661 && randomint <= 26214) // 6554 begin if(memb[j]>= (threshold[individual] + 1)) begin neuron = (neuron|(1<= 26215 && randomint <= 32767) // 6553 begin if(memb[j]>= (threshold[individual] + 2)) begin neuron = (neuron|(1<= minimum) begin memb[k] = memb[k] - leaking; end else begin memb[k] = minimum; end end // Ends for loop for leakage end // Ends task 1 //////////////////////////////////////////////////////////////// void task2(void) begin time2 = t2; // Left Motor // Stepper Motor moves 7.5 degree per step, so 48 steps / rev (360 / 7.5). Circumference of wheel is 15.9593 cm / rev (2 inch * 2.54 cm/ in * pi / 2). // Velocity = 15.9593 cm / rev * 1/48 rev / (t2 / 1000 s) = .332485 cm / (t2/1000 s) // Wire Configuration: Orange to C.0, Yellow to C.1, Brown to C.2, Black to C.3, Red and Green to 12V if(stepL==3) PORTC = (PORTC&0b11110000)|1; if(stepL==2) PORTC = (PORTC&0b11110000)|4; if(stepL==1) PORTC = (PORTC&0b11110000)|2; if(stepL==0) PORTC = (PORTC&0b11110000)|8; if(dirL) // 0 is forward, 1 is backward begin if(stepL > 0) stepL--; else stepL = 3; end else begin if(stepL < 3) stepL++; else stepL = 0; end end // Ends task 2 //////////////////////////////////////////////////////////////// void task3(void) begin time3 = t3; // Right Motor // Stepper Motor moves 7.5 degree per step, so 48 steps / rev (360 / 7.5). Circumference of wheel is 15.9593 cm / rev (2 inch * 2.54 cm/ in * pi / 2). // Velocity = 15.9593 cm / rev * 1/48 rev / (t2 / 1000 s) = .332485 cm / (t3/1000 s) // Wire Configuration: Orange to C.4, Yellow to C.5, Brown to C.6, Black to C.7, Red and Green to 12V if(stepR==3) PORTC = (PORTC&0b00001111)|16; if(stepR==2) PORTC = (PORTC&0b00001111)|64; if(stepR==1) PORTC = (PORTC&0b00001111)|32; if(stepR==0) PORTC = (PORTC&0b00001111)|128; if(!dirR) // 0 is forward, 1 is backward begin if(stepR > 0) stepR--; else stepR = 3; end else begin if(stepR < 3) stepR++; else stepR = 0; end end // Ends task 3 //////////////////////////////////////////////////////////////// void task4(void) begin time4 = t4; ////////////////////////////////////////Sensor Update////////////////////////////////////////////////// collision = 0; // Reset collision before recalculating for(x = 1; x <= ((numsensor / 3) + 1); x++) begin ADCSR.6=1; // Start Another Conversion while(ADCSR.6); // Debugging - testing ADCL for noise Ain2 = (ADCL>>6); Ain = ADCH; randomseed = randomseed + Ain; // Randomness is good Ain = (Ain2|(Ain<<2)); //Ain = ADCH; // Read ADC for Sensor 0 //voltage = ((float)((float)Ain*5.0/256.0)); // Convert to voltage, 256 instead of 1024 since only using 8 bits voltage = ((float)((float)Ain*5.0/1024.0)); // Depending on Voltage, set three bits per sensor // Debugging //if(x==6) printf("%ld %ld %12.8f\n\r", Ain, Ain2, voltage); // Debug ADC // NEED SENSOR ALGORITHM HERE // Sensor algorithm: // We use measured distance-voltage values of the IR sensors to establish a range for which a certain number of sensor bits will be set // There are three bits so there are four cases (000, 001, 011, 111) // 111 covers more distance than 011 // Sensoractivity, for fitness is also set // We also use a collision else statement to force fitness to zero in a collision (in addition to three bits being set in sensor) // To allow the robot to move when nothing is around, there will also be random spikes between sensor1not and sensor1top depending on the distance between them // The distance is used as a probability and we calculate a random value to decide if there will be a single spike if(x==1) // Calculate Sensor 1 Spikes begin if(voltage>=(float)sensor1top) begin probability = (((float)sensor1not - voltage)/((float)sensor1not-(float)sensor1top)); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000001); // Debug // printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000000); sensoractivity[0] = 0; // Debug //printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end else if(voltage<(float)sensor1top && voltage>=(float)sensor1mid) begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000001); sensoractivity[0] = .25; end else if(voltage<(float)sensor1mid && voltage>=(float)sensor1bot) begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000011); sensoractivity[0] = .50; end else if(voltage<(float)sensor1bot && voltage>(float)sensor1col) begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000111); sensoractivity[0] = .75; end else if(voltage<=(float)sensor1col) begin sensorread = ((sensorread&(0b111111111111000))|0b000000000000111); sensoractivity[0] = 1.0; collision = 1; end end // End Sensor 1 else if(x==2) // Calculate Sensor 2 Spikes begin if(voltage>=(float)sensor2top) begin probability = (((float)sensor2not - voltage)/((float)sensor2not-(float)sensor2top)); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin sensorread = ((sensorread&(0b111111111000111))|0b000000000001000); // Debug //printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin sensorread = ((sensorread&(0b111111111000111))|0b000000000000000); sensoractivity[1] = 0; // Debug //printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end else if(voltage<(float)sensor2top && voltage>=(float)sensor2mid) begin sensorread = ((sensorread&(0b111111111000111))|0b000000000001000); sensoractivity[1] = .25; end else if(voltage<(float)sensor2mid && voltage>=(float)sensor2bot) begin sensorread = ((sensorread&(0b111111111000111))|0b000000000011000); sensoractivity[1] = .50; end else if(voltage<(float)sensor2bot && voltage>(float)sensor2col) begin sensorread = ((sensorread&(0b111111111000111))|0b000000000111000); sensoractivity[1] = .75; end else if(voltage<=(float)sensor2col) begin sensorread = ((sensorread&(0b111111111000111))|0b000000000111000); sensoractivity[1] = 1.0; collision = 1; end end // End Sensor 2 else if(x==3) // Calculate Sensor 3 Spikes begin if(voltage>=(float)sensor3top) begin probability = (((float)sensor3not - voltage)/((float)sensor3not-(float)sensor3top)); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin sensorread = ((sensorread&(0b111111000111111))|0b000000001000000); // Debug //printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin sensorread = ((sensorread&(0b111111000111111))|0b000000000000000); sensoractivity[2] = 0; // Debug //printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end else if(voltage<(float)sensor3top && voltage>=(float)sensor3mid) begin sensorread = ((sensorread&(0b111111000111111))|0b000000001000000); sensoractivity[2] = .25; end else if(voltage<(float)sensor3mid && voltage>=(float)sensor3bot) begin sensorread = ((sensorread&(0b111111000111111))|0b000000011000000); sensoractivity[2] = .50; end else if(voltage<(float)sensor3bot && voltage>(float)sensor3col) begin sensorread = ((sensorread&(0b111111000111111))|0b000000111000000); sensoractivity[2] = .75; end else if(voltage<=(float)sensor3col) begin sensorread = ((sensorread&(0b111111000111111))|0b000000111000000); sensoractivity[2] = 1.0; collision = 1; end end // End Sensor 3 else if(x==4) // Calculate Sensor 4 Spikes begin if(voltage>=(float)sensor4top) begin probability = (((float)sensor4not - voltage)/((float)sensor4not-(float)sensor4top)); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin sensorread = ((sensorread&(0b111000111111111))|0b000001000000000); // Debug //printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin sensorread = ((sensorread&(0b111000111111111))|0b000000000000000); sensoractivity[3] = 0; // Debug //printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end else if(voltage<(float)sensor4top && voltage>=(float)sensor4mid) begin sensorread = ((sensorread&(0b111000111111111))|0b000001000000000); sensoractivity[3] = .25; end else if(voltage<(float)sensor4mid && voltage>=(float)sensor4bot) begin sensorread = ((sensorread&(0b111000111111111))|0b000011000000000); sensoractivity[3] = .50; end else if(voltage<(float)sensor4bot && voltage>(float)sensor4col) begin sensorread = ((sensorread&(0b111000111111111))|0b000111000000000); sensoractivity[3] = .75; end else if(voltage<=(float)sensor4col) begin sensorread = ((sensorread&(0b111000111111111))|0b000111000000000); sensoractivity[3] = 1.0; collision = 1; end end // End Sensor 4 else if(x==5) // Calculate Sensor 5 Spikes begin if(voltage>=(float)sensor5top) begin probability = (((float)sensor5not - voltage)/((float)sensor5not-(float)sensor5top)); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin sensorread = ((sensorread&(0b000111111111111))|0b001000000000000); // Debug //printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin sensorread = ((sensorread&(0b000111111111111))|0b000000000000000); sensoractivity[4] = 0; // Debug //printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end else if(voltage<(float)sensor5top && voltage>=(float)sensor5mid) begin sensorread = ((sensorread&(0b000111111111111))|0b001000000000000); sensoractivity[4] = .25; end else if(voltage<(float)sensor5mid && voltage>=(float)sensor5bot) begin sensorread = ((sensorread&(0b000111111111111))|0b011000000000000); sensoractivity[4] = .50; end else if(voltage<(float)sensor5bot && voltage>(float)sensor5col) begin sensorread = ((sensorread&(0b000111111111111))|0b111000000000000); sensoractivity[4] = .75; end else if(voltage<=(float)sensor5col) begin sensorread = ((sensorread&(0b000111111111111))|0b111000000000000); sensoractivity[4] = 1.0; collision = 1; end end // End Sensor 5 else if(x==6) // Calculate Sensor 6 Noises begin if(voltage >= (float)sensor6bot && voltage<=(float)sensor6top) begin probability = (((float)sensor6top - voltage)/((float)sensor6top-(float)sensor6bot)); srand(probability*1000); randomprob = (float)rand()/32767.0; if(randomprob <= probability) begin //sensorread = ((sensorread&(0b000111111111111))|0b001000000000000); // Debug //printf("hi %5.2f %5.2f\n\r", randomprob, probability); end else begin //sensorread = ((sensorread&(0b000111111111111))|0b000000000000000); //sensoractivity[4] = 0; // Debug // printf("YES %5.2f %5.2f\n\r", randomprob, probability); end end end // End Sensor 6 (noise) // Debugging //sensorread = (~0); if(x>=((numsensor / 3) + 1)) begin ADMUX = (ADMUX&(0b11100000)|0); end else begin ADMUX = (ADMUX&(0b11100000)|x); // Set to read Sensor 1, Take ADMUX and zero out last 5 bits, and set to the next sensor coming in end end // Ends for loop for sensor update //randomseed = randomseed/(numsensor/3); //srand(randomseed); // Calculate max sensor actiivty sensormaxactivity = sensoractivity[0]; for(g = 1; g < (numsensor/3); g++) begin if(sensoractivity[g] > sensormaxactivity) begin sensormaxactivity = sensoractivity[g]; end end // Debugging // This is the way to get the most random bits in, based on noise, will OR with regular senorread to get sensor values when plugged back into robot sensorread = (sensorread|(((rand()%2) << 0)|((rand() %2) << 3)|((rand() %2) << 6)|((rand() %2) << 9)|((rand() %2) << 12))); //sensorread = (((rand()%2) << 0)|((rand() %2) << 3)|((rand() %2) << 6)|((rand() %2) << 9)|((rand() %2) << 12)); //collision = 0; //sensormaxactivity = 0; //printf("%x\n\r", sensorread); // Only do Motor Speed Update every 160 ms, this allows for max 8 cm/s at least one movemnet since it takes 41ms to execute if(tsk4cnt==0) begin ///////////////////////////////////////Motor Speed Update////////////////////////////////////////////// // Motor speed for next 20ms is for each motor: spikesforward-spikesbackward scaled to [-8 cm/s, 8 cm/s] // Velocity will be determined by time of each step which is equal to .332485 cm / (spikesforward-spikesbackward) // Ie. this is initialized to .332485 cm / 3.32483 cm / s = .1 s = 100 ms // Concern here is that time will always be significantly higher than next motor udpate time, risk? //printf("%li %li %li %li %ld\n\r", spikeFL, spikeBL, spikeFR, spikeBR, maxspikes); if(spikeFL >= spikeBL) // If more forward then back, direction is forward begin // Debugging dirL = 0; // Multiply by 8.0 since maxspikes is actually maxspikes * 2 since if a neuron does spike it won't spike twice in a row because of the refractory period v2 = ((float)((float)((float)spikeFL - (float)spikeBL)/(float)maxspikes) * 2.0 * 8.0); // Debugging (comment out t2 change) t2 = (0.332485/v2*1000.0); time2 = t2; end else begin // Debugging dirL = 1; v2 = ((float)((float)((float)spikeBL - (float)spikeFL)/(float)maxspikes) * 2.0 * 8.0); // Debugging t2 = (0.332485/v2*1000.0); time2 = t2; end if(spikeFR >= spikeBR) // If more forward then back, direction is forward begin // Debugging dirR = 0; v3 = ((float)((float)((float)spikeFR - (float)spikeBR)/(float)maxspikes) * 2.0 * 8.0); // Debugging t3 = (0.332485/v3*1000.0); time3 = t3; end else begin // Debugging dirR = 1; v3 = ((float)((float)((float)spikeBR - (float)spikeFR)/(float)maxspikes) * 2.0 * 8.0); // Debugging t3 = (0.332485/v3*1000.0); time3 = t3; end // End Motor Speed update // Set tsk4cnt to be the greater of the times 350>tsk4cnt>60 if(t2 > t3 && t2 <=350) begin tsk4cnt = t2+10; end else if(t3 > t2 && t3 <=350) begin tsk4cnt = t3+10; end else tsk4cnt = 60; //printf("%li %li\n\r", t2, t3); end // End Motor Speed if statement // Fitness Calculation if(dirL==0 && dirR==0 && collision==0) // As long as going forward on both wheels and no collision begin if(v2>=v3) begin newfitness = newfitness + (v2+v3)*(1.0-((v2-v3)/8.0))*(1.0-(sensormaxactivity)); // Need to add something to sensor //change = (v2+v3)*(1.0-((v2-v3)/8.0))*(1.0-(sensormaxactivity)); //printf("%5.2f", change); end else begin newfitness = newfitness + (v2+v3)*(1.0-((v3-v2)/8.0))*(1.0-(sensormaxactivity)); // Need to add something to sensor //change = (v2+v3)*(1.0-((v3-v2)/8.0))*(1.0-(sensormaxactivity)); //printf("%5.2f", change); end end else if((dirL==1 || dirR==1) && collision==0) begin newfitness = newfitness + 0; //printf("YES\n\r"); end else if(collision==1) begin newfitness = newfitness + 0; //printf("NO\n\r"); end // Ready sensor and motor update sensorflag = 1; end // Ends task 4 //////////////////////////////////////////////////////////////// void task5(void) begin time5 = t5; // Fitness Check - If not greater return to old version in population // Debugging printf("%i %i %li %li %li %li %7.1f %7.1f %7.1f %7.1f %7.1f %7.1f %7.1f\n\r", individual, threshold[individual], totalspikeFL, totalspikeBL, totalspikeFR, totalspikeBR, newfitness, fitness[0], fitness[1], fitness[2], fitness[3], fitness[4], fitness[5]); worstindivid = 0; for(z = 1; z < numindivid; z++) // Calculate Worst Individual begin if(fitness[z] < fitness[worstindivid]) begin worstindivid = z; end end if(newfitness >= fitness[worstindivid] && newfitness>= fitness[individual]) begin keep = 1; fitness[individual] = newfitness; end else keep = 0; newfitness = 0; // Reset newfitness if(keep==0) begin signs[individual] = oldsigns; for(d = 0; d < numneuron; d++) begin nconnection[individual][d] = oldnconnection[d]; sconnection[individual][d] = oldsconnection[d]; end threshold[individual] = oldthreshold; end // Pick a random individual and mutate it individual = (rand() % numindivid); // Debugging //individual = 0; // Save old version oldsigns = signs[individual]; for(e = 0; e < numneuron; e++) begin oldnconnection[e] = nconnection[individual][e]; oldsconnection[e] = sconnection[individual][e]; end oldthreshold = threshold[individual]; // Mutate signs[individual] = (signs[individual]^(1<<(rand() % numneuron))); randn = (rand() % numneuron); rands = (rand() % numsensor); nconnection[individual][randn] = (nconnection[individual][randn]^(1<<(rand() % numneuron))); sconnection[individual][rands] = (sconnection[individual][rands]^(1<<(rand() % numsensor))); threshold[individual] = (threshold[individual]^(1<<(rand() % 4))); if(threshold[individual] > 6 || threshold[individual] < 2) threshold[individual] = oldthreshold; // Send signal evolveflag = 1; // Debugging //printf("%i %lx %lx %lx %lx\n\r", individual, signs[individual], nconnection[individual][randn], sconnection[individual][rands], sensor); end // Ends task 5 //////////////////////////////////////////////////////////////// void task6(void) begin time6 = t6; // Store Best Fitness Individual in EEPROM for(f = 0; f < numindivid; f++) begin if(fitness[f] >= bestfitness) begin bestfitness = fitness[f]; bestindivid = f; end // Debugging //printf("%li\n\r", bestfitness); end // STORE BEST INDIVID/FITNESS/GENETIC STRING IN EEPROM // EEPROM STUFF for(ii = 0; ii < numindivid; ii++) begin // Save best individual if(ii==bestindivid) begin eebestsigns = signs[ii]; for(zz = 0; zz < numneuron; zz++) begin eebestnconnection[zz] = nconnection[ii][zz]; eebestsconnection[zz] = sconnection[ii][zz]; end eebestthreshold = threshold[ii]; eebestfitness = fitness[ii]; end eesigns[ii] = signs[ii]; for(jj = 0; jj < numneuron; jj++) begin eenconnection[ii][jj] = nconnection[ii][jj]; eesconnection[ii][jj] = sconnection[ii][jj]; end eefitness[ii] = fitness[ii]; eethreshold[ii] = threshold[ii]; end eebestfitnessmem[eeindex] = bestfitness; eeindex = eeindex+1;; // Index of fitness values/replacements end // Ends task 6 //////////////////////////////////////////////////////////////// void main(void) begin initialize(); while(1) begin if (time1==0) task1(); if (time2==0) task2(); if (time3==0) task3(); if (time4==0) task4(); if (time5==0) task5(); if (time6==0) task6(); end end //////////////////////////////////////////////////////////////// void initialize(void) begin // PORTS DDRC = 0xff; // PORTC is set to output for motor control PORTC = 0x00; // TIMERS // timer0 is a 1 ms timer OCR0 = 250; // 1 ms ((1 / 16 MHZ) * 64 * 250 = 1 ms) TIMSK = 2; // Timer0 Compare Match Interrupt Enable TCCR0 = 0b00001011; // Prescalar set to 64, Clear on Compare Match //HYPERTERM //serial setup for debugging using printf, etc. UCSRB = 0x18 ; UBRRL = 103 ; printf("\r\nStarting...\r\n"); // ADC // ADC will be read by watching bit 6 in ADCSR, the ADSC bit, which is cleared when a conversion is finished ADMUX = 0b01100000; // Use AVCC (5V), Left Aligned, Start with ADC0 ADCSR = 0b10000111; // ADC Enabled, Not Started First Conversion, No Interrupt, Prescaler of 128 // Debug - Speed up ADC to get some noise! //ADCSR = 0b10000100; // Generate Random Seed randomseed = 0; for(y = 1; y <= ((numsensor / 3)+1); y++) begin ADCSR.6=1; // Start Another Conversion while(ADCSR.6); Ain = ADCH; // Read ADC for Sensor 0 randomseed = randomseed + Ain; if(y==6) // Calculate Sensor 6 Noises begin if(voltage >= (float)sensor6bot && voltage<=(float)sensor6top) begin //probability = (((float)sensor6top - voltage)/((float)sensor6top-(float)sensor6bot)); //srand(probability*1000); end end if(y>=((numsensor / 3)+1)) begin ADMUX = (ADMUX&(0b11100000)|0); end else begin ADMUX = (ADMUX&(0b11100000)|y); // Set to read Sensor 1, Take ADMUX and zero out last 5 bits, and set to the next sensor coming in end end randomseed = randomseed/(numsensor/3); srand(randomseed); // Variables time1 = t1; t2 = 100; // 100 ms is initialized value for how long in between each step of left/right motors (10 steps / second) t3 = 100; // Each step is 7.5 degrees so this will give 3.32483 cm/second (10 steps / second * 7.5 degree / step * .044331 cm / degree) time2 = t2; time3 = t3; time4 = t4; time5 = t5; time6 = t6; sensorflag = 0; evolveflag = 0; // Flag that indicates a mutation has just occurred tsk4cnt = 100; // Counter for task 4 to allow us to update motors every 60 ms to allow for actual movement // Neuron Variables neuron = 0; // Output of Neurons (1 is spike, 0 is no spike), ie. 0b11001100 means neurons 2, 3, 6, 7 are spiking, sensors 0, 1, 4, 5 are not sensor = 0; // Output of Sensors (1 is spike, 0 is no spike), ie. 0b11001100 means sensors 2, 3, 6, 7 are spiking, sensors 0, 1, 4, 5 are not individual = (rand() % (numindivid)); // Current Individual (0 - 5) // The population will be initialized using random numbers and all fitness values will be set to zero // Use EEPROM if already started a population if(eeindex==0) begin for(b = 0; b < numindivid; b++) begin signs[b] = (rand() % (1<= worst individuals fitness collision = 0; // Indicates a collision for fitness calculation (0 no collision, 1 collision) sensormaxactivity = 0; // Most active sensor value for fitness calculation (5 values for this based on most active sensor: 0 if nothing, .25 if between top and mid, .5 if between mid and bot, .75 if between bot and col, 1 if below or equal to col) for(h = 0; h < (numsensor/3); h++) begin sensoractivity[h] = 0; // Activity of 5 sensors based on values above to be maxed end; worstindivid = 0; // Worst indiviudal used to determine keep randn = 0; // Random Variables to access blocks in random individual for mutation rands = 0; change = 0; // Change in fitness (debugging) // Fitness Store variables bestfitness = 0; // Best Fitness so far bestindivid = 0; // Best Individual // Print out Current EEPROM Best Individual (if avaliable) // Print out Current EEPROM Best Fitness (if avaliable) if(eeindex!=0) begin printf("\n\r"); printf("%lx\n\r", eebestsigns); delay_ms(10); for(yy = 0; yy < numneuron; yy++) begin printf("%lx\n\r", eebestnconnection[yy]); delay_ms(2); printf("%lx\n\r", eebestsconnection[yy]); end printf("%i\n\r", eebestthreshold); delay_ms(10); printf("%li\n\r", eebestfitness); delay_ms(10); printf("\n\r"); for(xx = 0; xx < eeindex; xx++) begin printf("%li\n\r", eebestfitnessmem[xx]); delay_ms(2); end end //crank up the ISRs #asm sei #endasm end