Add robot.c version 3.6

Author: mesheets

robot.c

@@ -0,0 +1,204 @@
+/* robot3_6.c
+*
+* These messages are also used by the Java program robot3.Controller
+*
+* Messages, PC => RCX
+* * R = reset, motor powers to 0 and motors to float
+* S = stop (brake, full voltage to motors)
+* Mxaybzc = Set motor state and speed, x=speed for A, a=state for A,
+* y=speed for B, b=state for B...
+* Ax = set motor A's power to the value of x (binary, 0..255)
+* Bx = set motor B's power to the value of x (binary)
+* Cx = set motor C's power to the value of x (binary)
+* 1x = set motor A state to x, (0=off (float), 1=forward, 2=reverse, 3=brake)
+* defined in robot.Controller.[FLOAT|FORWARD|REVERSE|BRAKE] )
+* 2x = set motor B state to x
+* 3x = set motor C state to x
+*
+* -----------------------------------------------------------------------------
+*
+*  by Motti Frimer 2003
+*/
+
+#include <conio.h>
+#include <unistd.h>
+#include <string.h>
+#include <lnp.h>
+#include <dmotor.h>
+#include <dsensor.h>
+#include <time.h>
+#include <lnp-logical.h>
+
+unsigned char sensor_data[18];
+
+unsigned char motor_A_speed;
+unsigned char motor_A_dir;
+unsigned char motor_B_speed;
+unsigned char motor_B_dir;
+unsigned char motor_C_speed;
+unsigned char motor_C_dir;
+
+unsigned char index;
+
+#define Abs(x)	((x >= 0) ? x : -x)
+
+int  index_out_A;
+int  index_out_B;
+
+int  rotation_sens_old_A;
+int  rotation_sens_old_B;
+
+unsigned int  wait;
+int counter;
+
+
+/* Brakes with all the motors */
+void stop_robot() {
+	motor_a_dir(brake);
+	motor_b_dir(brake);
+	motor_c_dir(brake);
+	motor_a_speed(255);
+	motor_b_speed(255);
+	motor_c_speed(255);
+}
+
+void my_integrity_handler(const unsigned char *data, unsigned char len)
+{
+	if(data[0]=='S') {
+		stop_robot();
+	}
+	else if(data[0]=='R') {
+		motor_a_dir(off);
+		motor_b_dir(off);
+		motor_c_dir(off);
+		motor_a_speed(0);
+		motor_b_speed(0);
+		motor_c_speed(0);
+	}
+	else if(data[0]=='M') {
+		motor_a_dir(data[2]);
+		motor_b_dir(data[4]);
+		motor_c_dir(data[6]);
+		motor_a_speed(data[1]);
+		motor_b_speed(data[3]);
+		motor_c_speed(data[5]);
+	}
+	else if(data[0]=='A') {
+        	motor_a_speed(data[1]);
+                motor_A_speed = data[1];
+	}
+	else if(data[0]=='B') {
+		motor_b_speed(data[1]);
+                motor_B_speed = data[1];
+	}
+	else if(data[0]=='C') {
+		motor_c_speed(data[1]);
+		motor_C_speed = data[1];
+	}
+	else if(data[0]=='1') {
+		motor_a_dir(data[1] );
+                motor_A_dir = data[1];
+	}
+	else if(data[0]=='2') {
+		motor_b_dir(data[1] );
+                motor_B_dir = data[1];
+	}
+	else if(data[0]=='3') {
+		motor_c_dir(data[1] );
+		motor_C_dir = data[1];
+	}
+	else if(data[0]=='I') {
+		index = data[1];
+	}
+
+}
+
+
+
+int main()
+{
+        motor_A_speed = 0;
+        motor_A_dir = 0;
+        motor_B_speed = 0;
+        motor_B_dir = 0;
+	motor_C_speed = 0;
+	motor_C_dir = 0;
+
+	index = 0;
+	index_out_A = 0;
+	index_out_B = 0;
+
+	counter=0;
+
+	rotation_sens_old_A = 0;
+	rotation_sens_old_B = 0;
+
+	wait = 500;
+
+
+	ds_active(&SENSOR_1);  /* activates sensor on port 1 */
+	ds_rotation_on(&SENSOR_1); /* activates rotation sensor on port 1 */
+	ds_rotation_set(1,10); /* sets initial value that sensor returns */
+
+	ds_active(&SENSOR_2);  /* activates sensor on port 2 */
+	ds_rotation_on(&SENSOR_2); /* activates rotation sensor on port 2 */
+	ds_rotation_set(2,10); /* sets initial value that sensor returns */
+
+
+
+	lnp_integrity_set_handler(my_integrity_handler);
+	lnp_logical_range(0);  // sets the transmitter mode to the short range
+
+	while(1)
+	{
+
+
+
+		rotation_sens_old_A = ROTATION_1;
+		rotation_sens_old_B = ROTATION_2;
+
+		msleep(wait);
+
+		sensor_data[0]='s';
+		sensor_data[1]= (unsigned char)(ROTATION_1 & 0xFF);
+		sensor_data[2]= (unsigned char)((ROTATION_1 & 0xFF00) >> 8);
+                sensor_data[3]= (unsigned char)(ROTATION_2 & 0xFF);
+		sensor_data[4]= (unsigned char)((ROTATION_2 & 0xFF00) >> 8);
+                sensor_data[5]= (unsigned char)(TOUCH_3 & 0xFF);
+		sensor_data[6]= (unsigned char)((TOUCH_3 & 0xFF00) >> 8);
+
+                /* Motor data */
+                sensor_data[7]= motor_A_speed;
+		sensor_data[8]= motor_A_dir;
+                sensor_data[9]= motor_B_speed;
+		sensor_data[10]= motor_B_dir;
+		sensor_data[11]= motor_C_speed;
+		sensor_data[12]= motor_C_dir;
+
+
+		/* Multiply by 2=(1/(wait/1000)) in order to compute the speed per sec */
+               
+		index_out_B=Abs(( ROTATION_2 - rotation_sens_old_B )*2);
+                index_out_A=Abs(( ROTATION_1 - rotation_sens_old_A )*2);
+                sensor_data[13]= (unsigned char)(index_out_A & 0xFF);
+		sensor_data[14]= (unsigned char)((index_out_A & 0xFF00) >> 8);
+		sensor_data[15]= (unsigned char)(index_out_B & 0xFF);
+		sensor_data[16]= (unsigned char)((index_out_B & 0xFF00) >> 8);
+
+		lnp_integrity_write(sensor_data, sizeof(sensor_data));
+
+		if(TOUCH_3!=0)    //if sensor has been touched
+		{
+			motor_a_dir(rev);
+			motor_b_dir(fwd);
+			sleep(3);
+			motor_a_dir(brake);
+			motor_b_dir(brake);
+
+		}
+
+
+
+	}
+	return 0;
+}