robotc

commit cb5cbde300c7be8d7f6b9b657356f1f569a39ccb
Author: Jul <jul@9o.is>
Date:   Tue, 18 Dec 2012 14:09:32 -0500

init

Diffstat:
A
deadreckoning.c
 | 
35
+++++++++++++++++++++++++++++++++++
A
linetracking.c
 | 
24
++++++++++++++++++++++++
A
potential.c
 | 
148
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
A
subsump2.png
 | 
0

A
subsumptionLevel1.c
 | 
152
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

5 files changed, 359 insertions(+), 0 deletions(-)
diff --git a/deadreckoning.c b/deadreckoning.c
@@ -0,0 +1,34 @@
+// circumference with 56 mm diameter wheel
+float revolution = 3.1415 * 56; //175.924
+float meter = revolution * 5.6842;
+
+
+void turn90() {
+  nMotorEncoder[motorB] = 0;
+  nMotorEncoder[motorC] = 0;
+ while(nMotorEncoder[motorC] < revolution * 3.2) {
+         motor[motorC] = 100;
+         motor[motorB] = 0;
+ }
+}
+
+
+void move(int distance) {
+  nMotorEncoder[motorB] = 0;
+  nMotorEncoder[motorC] = 0;
+ while(nMotorEncoder[motorB] < distance) {
+         motor[motorC] = 100;
+         motor[motorB] = 100;
+        }
+}
+
+
+task main() {
+
+
+ // complete 4 sides of square
+ for(int i=0; i < 4; i++) {
+          move(meter/2); //move half a meter
+          turn90();
+ }
+}
+\ No newline at end of file
diff --git a/linetracking.c b/linetracking.c
@@ -0,0 +1,23 @@
+//*!!Sensor,    S2,          lightsensor, sensorLightActive,      ,              !!*//
+//*!!                                                                            !!*//
+//*!!Start automatically generated configuration code.                           !!*//
+const tSensors lightsensor          = (tSensors) S2;   //sensorLightActive  //*!!!!*//
+//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//
+
+
+// This program uses the light sensor to track the left side of a line
+
+
+task main() {
+        int threshold = 43;
+        ClearTimer(T1);
+        while(time1[T1] < 3000) {
+          if(SensorValue(lightsensor) < threshold) {
+                        motor[motorC]=0;
+                        motor[motorB]=50;
+                } else {
+                        motor[motorC]=50;
+                        motor[motorB]=0;
+                }
+        }
+}
+\ No newline at end of file
diff --git a/potential.c b/potential.c
@@ -0,0 +1,148 @@
+#pragma config(Sensor, S1, touchSensor, sensorTouch)
+#pragma config(Sensor, S2, irSensor, sensorHiTechnicIRSeeker1200)
+#pragma config(Sensor, S3, sonarSensor, sensorSONAR)
+#pragma config(Sensor, S4, analogSensor, sensorAnalogActive)
+
+// 10 inches or less is considered near
+int NEAR_SONAR = 25;
+int NEAR_ANALOG = 1020;
+int SPEED = 50;
+
+typedef struct {
+  float dir;
+  float mag;
+} VECTOR;
+
+void vectorSum(VECTOR const& a, VECTOR const& b, VECTOR& sum)
+{
+  float atheta = PI * a.dir / 180;
+  float btheta = PI * b.dir / 180;
+  float ax = a.mag * cos(atheta);
+  float ay = a.mag * sin(atheta);
+  float bx = b.mag * cos(btheta);
+  float by = b.mag * sin(btheta);
+  float rx = ax + bx;
+  float ry = ay + by;
+  float r = sqrt(rx * rx + ry * ry);
+
+  //nxtDisplayCenteredTextLine(5, "%f %f", ax, bx);
+
+  sum.mag = r;
+
+  if (rx == 0)
+    sum.dir = (ry > 0 ? 90 : (ry < 0 ? -90 : 0));
+  else
+  {
+    sum.dir = 180 * atan(ry / rx) / PI; // arctan approximation
+    if (rx < 0) {
+      if (ry < 0)
+        sum.dir = sum.dir - 180;
+      else
+        sum.dir = 180 + sum.dir;
+    }
+  }
+  //nxtDisplayCenteredTextLine(6, "%f %f", sum.mag, sum.dir);
+}
+
+/* Checks if the EOPD sensor detects an obstacle. */
+bool obstacleNearAnalog() {
+  return (SensorValue[analogSensor] < NEAR_ANALOG) ? true : false;
+}
+
+/* Checks if the SONAR sensor detects an obstacle. */
+bool obstacleNearSonar() {
+  return (SensorValue[sonarSensor] < NEAR_SONAR) ? true : false;
+}
+
+/*
+ * Creates a Vector that runs away from obstacles
+ * detected by SONAR and EOPD sensor.
+ *
+ * Assumes the SONAR is facing 45 degrees left of the robot.
+ * Assumes the EOPD is facing 45 degrees right of the robot.
+ */
+void runaway(VECTOR& v) {
+  bool sonar = obstacleNearSonar();
+  bool analog = obstacleNearAnalog();
+
+  VECTOR analogVector;
+  analogVector.mag = (analog) ? 1.0 : 0;
+  analogVector.dir = (analog) ? 135 : 0;
+
+  VECTOR sonarVector;
+  sonarVector.mag = (sonar) ? 1.0 : 0;
+  sonarVector.dir = (sonar) ? -135 : 0;
+
+  vectorSum(analogVector, sonarVector, v);
+}
+
+/*
+ * Directs Vector v to the IR signal.
+ * Moves left or right by 30 degrees
+ * if no IR signal is detected.
+ */
+void move2Goal(VECTOR& v) {
+  int irSensor = SensorValue[S2];
+  v.mag = 1.0;
+
+  if(irSensor == 0)
+    v.dir = (random(1) == 1) ? 30 : -30;
+  else
+    v.dir = (5 - irSensor) * 22; //22 degrees
+}
+
+/*
+ * Turns the robot by a rotation value.
+ * A rotation value of 348 is equivalent
+ * to 180 degree turn.
+ */
+void turn(float rot)
+{
+  float dir = (rot * 348) / 180;
+  float speed = abs(dir / 5.8);
+  if (dir > 0)
+  {
+    motor[motorA] = -speed;
+    motor[motorC] = speed;
+    wait1Msec(500);
+  }
+  else if (dir < 0)
+  {
+    motor[motorA] = speed;
+    motor[motorC] = -speed;
+    wait1Msec(500);
+  }
+  motor[motorA] = 0;   //power level of 100.
+  motor[motorC] = 0;
+}
+
+/*
+ * Moves the robot forward by its speed and magnitude.
+ */
+void move(float mag) {
+  motor[motorA] = mag * SPEED;   //Motor A & C is at
+  motor[motorC] = mag * SPEED;   //power level of 100.
+  wait1Msec(500);
+}
+
+task main() {
+
+  VECTOR goalVector, repulseVector, attractVector;
+
+  nxtDisplayCenteredTextLine(0, "Running...");
+  while(true) {
+    move2Goal(goalVector);
+    nxtDisplayCenteredTextLine(1, "%f %f", goalVector.dir, goalVector.mag);
+
+    runaway(repulseVector);
+    nxtDisplayCenteredTextLine(2, "%f %f", repulseVector.dir, repulseVector.mag);
+
+    vectorSum(goalVector, repulseVector, attractVector);
+    nxtDisplayCenteredTextLine(3, "%f %f", attractVector.dir, attractVector.mag);
+
+    // now move robot
+    turn(attractVector.dir);
+    move(attractVector.mag);
+  }
+
+}
diff --git a/subsump2.png b/subsump2.png
Binary files differ.
diff --git a/subsumptionLevel1.c b/subsumptionLevel1.c
@@ -0,0 +1,152 @@
+#pragma config(Sensor, S2, IRSeeker2, sensorHiTechnicIRSeeker1200)
+#pragma config(Sensor, S3, sonarSensor, sensorSONAR)
+#pragma config(Sensor, S4, analogSensor, sensorAnalogActive)
+//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
+
+int NEAR_SONAR = 25;
+int NEAR_ANALOG = 1020;
+
+/* for vector objects */
+struct {
+float dir;
+float mag;
+} VECTOR;
+
+/* Avoids obstacles. */
+VECTOR avoid(VECTOR& force);
+
+/* Checks if the EOPD sensor detects an obstacle. */
+bool obstacleNearAnalog();
+
+/* Checks if the SONAR sensor detects an obstacle. */
+bool obstacleNearSonar();
+
+/* Moves randomly to any direction. */
+void wander(VECTOR& force);
+
+/* converts raw sensor reading into force vector */
+void feelForce(int percept, VECTOR& force);
+
+/* converts raw sensor percept into boolean */
+bool collide(int percept);
+
+/* converts a force vector into a heading vector */
+void runaway(VECTOR const& force, VECTOR& heading);
+
+/* directs actuators to move forward at power proportional to mag */
+void forward(bool halt, float mag);
+
+/* directs actuators to turn by dir degrees */
+void turn(float dir);
+
+/* robot control loop */
+task main() {
+  VECTOR force;
+  VECTOR heading;
+
+  while (true) {
+    // print diagnostic text here
+    float percept = SensorValue[IRSeeker2];
+    bool halt = collide(percept);
+    avoid(force);
+    feelForce(percept, force);
+    runaway(force, heading);
+
+    // print diagnostic heading dir and mag here
+    turn(heading.dir);
+    forward(halt, heading.mag);
+  }
+}
+
+/* Avoids obstacles. */
+void avoid(VECTOR& force) {
+  float sonar = obstacleNearSonar();
+  float analog = obstacleNearAnalog();
+ 
+  if(sonar && analog) {
+    force.mag = 1.0;
+    force.dir = 180;
+  } else if(sonar) {
+    force.mag = 1.0;
+    force.dir = 135;
+  } else if(analog) {
+    force.mag = 1.0;
+    force.dir = -135;
+  } 
+}
+
+/* Checks if the EOPD sensor detects an obstacle. */
+bool obstacleNearAnalog() {
+  return (SensorValue[analogSensor] < NEAR_ANALOG) ? true : false;
+}
+
+/* Checks if the SONAR sensor detects an obstacle. */
+bool obstacleNearSonar() {
+  return (SensorValue[sonarSensor] < NEAR_SONAR) ? true : false;
+}
+
+/*
+ * Is it facing the IR?
+ */
+bool collide(int percept){
+  return percept != 0;
+}
+
+/*
+ * Detect at what degree the IR is.
+ */
+void feelForce(int percept, VECTOR& force){
+  force.mag = (percept == 0 ? 100 : 0);
+
+  if(irSensor == 0)
+    wander(force);
+  else
+    force.dir = (5 - irSensor) * 22; //22 degrees
+}
+
+/* Moves randomly to any direction. */
+void wander(VECTOR& force) {
+  force.dir = (random(1) == 1) ? 30 : -30;
+}
+
+/*
+ * Set the heading vector as force vector.
+ */
+void runaway(VECTOR const& force, VECTOR& heading){
+  heading.mag = force.mag;
+  heading.dir = force.dir;
+}
+
+/*
+ * Move forward if not in halt.
+ */
+void forward(bool halt, float mag) {
+  if (halt) {
+    motor[motorA] = 0;
+    motor[motorC] = 0;
+  } else {
+    motor[motorA] = mag;
+    motor[motorC] = mag;
+  }
+}
+
+/*
+* dir in degrees
+* 348 rotation = 180 degrees
+*/
+void turn(float dir){
+  motor[motorA] = 0;
+  motor[motorC] = 0;
+  float rot = (dir * 348) / 180;
+  if(rot < 0){
+    while(nMotorEncoder[motorA] < -(rot)){
+      motor[motorA] = 50;
+      motor[motorC] = -50;
+    }
+  } else {
+    while(nMotorEncoder[motorA] < rot){
+      motor[motorA] = -50;
+      motor[motorC] = 50;
+    }
+  }
+}