Code: #pragma config(Sensor, S1, color, sensorCOLORRED) #pragma config(Sensor, S2, touch, sensorTouch) #pragma config(Sensor, S4, sonar, sensorSONAR) #pragma config(Motor, motorA, ma, tmotorNormal, PIDControl, encoder) #pragma config(Motor, motorB, mb, tmotorNormal, PIDControl, encoder) #pragma config(Motor, motorC, mc, tmotorNormal, PIDControl, encoder) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// // Calibration values, these may be different for you const int blackValue = 220; const int whiteValue = 580; int index = 0; char arr[50];
void path(char a) { if(index==50) index=0; arr[index++]=a; //for(int i = index-1; i < sizeof(arr); i++){ if(arr[index-1] == 'b'){ //to delete 'b', and move everything over: index--; //for(int j = i; j < (sizeof(arr) - 1); j++){ arr[index] = '0';
//} } //} nxtDisplayTextLine(6, "Raw: %3d, %3d", index, a);
}
// Normalise a raw value, returns a value from 0 to 100 int normaliseReading(int rawValue) { // Anything less than the black value should return 0 if (rawValue <= blackValue) return 0;
// Anything brighter than the white value should return 100 else if (rawValue >= whiteValue) return 100;
// Anything else should be calculated in a scale from 0-100% else return ((long)(rawValue - blackValue) * 100) / (whiteValue - blackValue); } task main() { int rawValue = 0; int normalisedValue = 0;
while (true)
{
while(SensorValue(touch) == 0) //a while loop is declared with the touchsensor's value being 0 as it true condition { // Get the raw value from the sensor. rawValue = SensorRaw[color];
// Calculate the normalised value normalisedValue = normaliseReading(rawValue);
nxtDisplayTextLine(4, "Raw: %3d", rawValue); nxtDisplayTextLine(5, "Norm: %3d", normalisedValue); wait1Msec(100); rawValue = SensorRaw[color]; if (rawValue>=300) { if((SensorValue(sonar)>15)&&((SensorValue(sonar)<18))) { motor[mc]=30; motor[mb]=60; SensorValue(color); SensorValue(sonar); } else if(SensorValue(sonar)<11) { motor[mc]=60; motor[mb]=30; SensorValue(color); SensorValue(sonar); }
} else if (rawValue<300) { motor[mc]=60; motor[mb]=60; path('s'); SensorValue(color); SensorValue(sonar); if(SensorValue(sonar)>18) { motor[mc]=0; motor[mb]=60; wait1Msec(500); path('l');
SensorValue(color); SensorValue(sonar); } } } motor[mb] = -60; //motor A is run at a -75 power level motor[mc] = -60; wait1Msec(1000); motor[mb] = +60; //motor A is run at a -75 power level motor[mc] = -60; //motor B is run at a -75 power level wait1Msec(1000); //the program waits 1000 milliseconds before running further code path('b');
} } |