robot_project.nxc

//Authour: Greg Knoblauch
//NXC Code for the training of a neural network
//Convention: Left and Right refer to the SENSOR/MOTOR, NOT THE WIRING.


#define LeftMotor OUT_B
#define RightMotor OUT_C
#define LeftUltra S2
#define RightUltra S3
#define LeftMic S1
#define RightMic S4

int Sensitivity = 1; //Level 0 connection: Ultrasonic sensitivity. Default 1.
int LMic = 0; //Impact of the left microphone
int RMic = 0; //Impact of the right microhone
int ultraSensitivity = 60; // Pecentage of ultra reading that affects the motor speed
int micSensitivity = 40; // Pecentage of mic reading that affects the motor speed
int RightSpeed = 0; // Initalization of value for the speed of the right motor
int LeftSpeed = 0;// Initalization of value for the speed of the left motor


/*====="Level -1": Integration==================================================
Each of the terms (ultraSensitivity, mic) is part of a later
level's connection to the motors. See the main task to see their defaults.
On its own, this task does nothing. However, it will function at every level
without modification.
*/

task Drive(){
  while(true){
    //"Hearing/255" converts from responsive raw ultrasonic to % motor speed.d
    RightSpeed = ((SensorUS(RightUltra)*(ultraSensitivity)/255)+RMic) * (-1);
    LeftSpeed = ((SensorUS(LeftUltra)*(ultraSensitivity)/255)+LMic) * (-1);
  }
}

/*=====Level 0: Drive===========================================================
Feed the distance from each ultrasonic sensor to the motor. The robot is wired
contralaterally, and thus avoids all walls equally. As a result, when it reaches
a corner, it slows down and ends up stopping in the corner "for free".
*/

task DriveRight(){
  while(true){
    OnFwd(RightMotor, RightSpeed);
  }
}

task DriveLeft(){
  while(true){
    OnFwd(LeftMotor, LeftSpeed);
  }
}

/*=====Level 1: Hear===========================================================
Reads the decible level from both microphones. Decible devel are read in values 1-100.
They are multiplied by the mic sensitivity value then divided by 100 to get a vale from 0-1.
*/

task Hear(){ 
  while(true){
    LMic = Sensor(LeftMic)*micSensitivity/100;
    RMic = Sensor(RightMic)*micSensitivity/100;
  }
}

/*=====Level 2: Logging=========================================================
Simple Logginf Function to write all inputs and outputs to file. 
*/
byte handle;
long fileSize = 10000;
short bytesWritten;
string write;
string comma = ",";

task Logger(){
  DeleteFile("Example.txt");
  CreateFile("Example.txt", fileSize, handle);
  while(true){
    Wait(250);
    int left_mic = Sensor(LeftMic);
    int right_mic = Sensor(RightMic);
    int left_ultra = SensorUS(LeftUltra);
    int right_ultra = SensorUS(RightUltra);


    string left_eye_str = NumToStr(left_mic);
    string right_eye_str = NumToStr(right_mic);
    string left_ear_str = NumToStr(left_ultra);
    string right_ear_str = NumToStr(right_ultra);
    string left_motor_str = NumToStr(LeftSpeed);
    string right_motor_str = NumToStr(RightSpeed);

    write = StrCat(left_eye_str, comma, right_eye_str, comma, left_ear_str, comma, right_ear_str, comma, left_motor_str, comma,  right_motor_str);
    WriteLnString(handle, write, bytesWritten);
  }
  CloseFile(handle);
}



//=====Main Task================================================================

task main(){
  //Set up ultrasonic sensors and speed calculation weights.
  SetSensorLowspeed(LeftUltra);
  SetSensorMode(LeftUltra, SENSOR_MODE_RAW);

  SetSensorLowspeed(RightUltra);
  SetSensorMode(RightUltra, SENSOR_MODE_RAW);

  SetSensorType(LeftMic, SENSOR_TYPE_SOUND_DB);
  SetSensorMode(LeftMic, SENSOR_MODE_PERCENT);

  SetSensorType(RightMic, SENSOR_TYPE_SOUND_DB);
  SetSensorMode(RightMic, SENSOR_MODE_PERCENT);

  start Drive;
  start DriveRight;
  start DriveLeft;
  start Hear;
  start Logger;
}