int main()
{
char inp;
ClearTick();
result = BrickPiSetup();
// printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
motor1=PORT_B; //Select the ports to be used by the motors
motor2=PORT_C;
BrickPi.MotorEnable[motor1] = 1; //Enable the motors
BrickPi.MotorEnable[motor2] = 1;
result = BrickPiSetupSensors(); //Set up the properties of sensors for the BrickPi
//printf("BrickPiSetupSensors: %d\n", result);
BrickPi.Timeout=3000; //Set timeout value for the time till which to run the motors after the last command is pressed
BrickPiSetTimeout(); //Set the timeout
if(!result)
{
while(1)
{
scanf("%c",&inp); //Take input from the terminal
move_bot(inp); //Move the bot
if(inp=='t') //Increase the speed
{
printf("Speed: %d\n",speed);
if(speed >234)
speed=255;
else
speed=speed+10;
move_bot(cmd); //call move_bot() to update the motor values
}
else if(inp=='g') //Decrease the speed
{
printf("Speed: %d\n",speed);
if(speed<11)
speed=0;
else
speed=speed-10;
move_bot(cmd);
}
BrickPiUpdateValues(); //Update the motor values
usleep(10000); //sleep for 10 ms
}
}
return 0;
}
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
printf("BrickPiSetup Done!\n");
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType[US_PORT] = TYPE_SENSOR_EV3_GYRO_M3;
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
//usleep(5000000);
if(1){
usleep(10000);
while(1){
result = BrickPiUpdateValues();
if(!result){
val = BrickPi.Sensor[US_PORT];
val1 = val & 0xffff;
val2 = (val & 0xffff0000) >> 16;
//if(val>32000 && val!= 65532 && val != 65534 ) val = val-65535;
//if(val >= -1000 && val <=1000)
if(val!= -2 && val != -4){
if (val1 > 32000) val1 -= 65535;
if (val2 > 32000) val2 -= 65535;
printf("Results: %ld %ld \n", val1,val2 );
}
}
usleep(50000);
}
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType[PORT_1] = TYPE_SENSOR_TOUCH;
// BrickPi.SensorType[PORT_2] = TYPE_SENSOR_TOUCH;
// BrickPi.SensorType[PORT_3] = TYPE_SENSOR_TOUCH;
// BrickPi.SensorType[PORT_4] = TYPE_SENSOR_TOUCH;
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
if(!result){
usleep(10000);
while(1){
result = BrickPiUpdateValues();
if(!result){
// printf("Results: %3.1d %3.1d %3.1d %3.1d \n", BrickPi.Sensor[PORT_1], BrickPi.Sensor[PORT_2],BrickPi.Sensor[PORT_3], BrickPi.Sensor[PORT_4] );
printf("Results: %3.1d \n", BrickPi.Sensor[PORT_1] );
}
usleep(10000);
}
}
return 0;
}
int main() {
int result;
ClearTick();
printf("This program is free software. You can redistribute it and/or modify it under the terms of \nthe GNU General Public License as published by the Free Software Foundation; version 3 of the License. \nRead the license at: http://www.gnu.org/licenses/gpl.txt\n\n");
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
BrickPi.SensorType [I2C_PORT] = TYPE_SENSOR_I2C;
BrickPi.SensorI2CSpeed [I2C_PORT] = I2C_SPEED;
BrickPi.SensorI2CDevices [I2C_PORT] = 1;
BrickPi.SensorSettings [I2C_PORT][0] = 0;
BrickPi.SensorI2CAddr [I2C_PORT][0] = 0x02; //address for writing
BrickPiSetupSensors();
struct button b1;
b1=init_psp(b1);
while(1)
{
//Send 0x42 to get a response back
BrickPi.SensorI2CWrite [I2C_PORT][0] = 1; //number of bytes to write
BrickPi.SensorI2CRead [I2C_PORT][0] = 6; //number of bytes to read
BrickPi.SensorI2COut [I2C_PORT][0][0] = 0x42; //byte to write
BrickPiUpdateValues();
printf("%d %d %d\n",BrickPi.SensorI2CIn[I2C_PORT][0][0],BrickPi.SensorI2CIn[I2C_PORT][0][1],BrickPi.SensorI2CIn[I2C_PORT][0][2]);
b1=upd(b1,I2C_PORT); //Update the button values
show_val(b1); //#Show the values
//To use the button values in you own program just call it,
//eg x=b.ljx will fetch and store the value of the Left Joystick position in the X-axis in the variable x
b1=init_psp(b1); //Initialize all buttons to 0
usleep(100000); //Give a delay of 100ms
}
return 0;
}
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType [I2C_PORT] = TYPE_SENSOR_I2C;
BrickPi.SensorI2CSpeed [I2C_PORT] = I2C_SPEED;
BrickPi.SensorI2CDevices [I2C_PORT] = 1;
/* Getting continuous values from a HMC5883L:
1. Write CRA (00) – send 0x3C 0x00 0x70 (8-average, 15 Hz default, normal measurement)
2. Write CRB (01) – send 0x3C 0x01 0xA0 (Gain=5, or any other desired gain)
3. Write Mode (02) – send 0x3C 0x02 0x00 (Continuous-measurement mode)
4. Wait 6 ms or monitor status register or DRDY hardware interrupt pin
5. Loop
Send 0x3D 0x06 (Read all 6 bytes. If gain is changed then this data set is using previous gain)
Convert three 16-bit 2’s compliment hex values to decimal values and assign to X, Z, Y, respectively.
Send 0x3C 0x03 (point to first data register 03)
Wait about 67 ms (if 15 Hz rate) or monitor status register or DRDY hardware interrupt pin
End_loop */
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DCOM] = 0;
BrickPi.SensorI2CAddr [I2C_PORT][I2C_DEVICE_DCOM] = 0x3C; //address for writing
if(BrickPiSetupSensors())
return 0;
//1
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_DCOM] = 2; //number of bytes to write
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DCOM] = 0; //number of bytes to read
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][0] = 0x00; //byte to write
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][1] = 0x70; //byte to write
if(BrickPiUpdateValues()) //writing
return 0;
if(!(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DCOM))) //BrickPi.Sensor[PORT] has an 8 bit number consisting of success(1) or failure(0) on all ports in bus
return 0;
//2
//we're writing 2 bytes again, so there's no need to redefine number of butes
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][0] = 0x01;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][1] = 0xA0;
if(BrickPiUpdateValues())
return 0;
if(!(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DCOM)))
return 0;
//3
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][0] = 0x02;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][1] = 0x00;
if(BrickPiUpdateValues())
return 0;
if(!(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DCOM)))
return 0;
usleep(100000);
//loop
//writing 0x03 at address 0x3C to move the register to the first byte of data(0x03)
//and then reading 6 bytes from 0x3D
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DCOM] = BIT_I2C_SAME;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_DCOM] = 1;
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DCOM] = 6;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DCOM][0] = 0x03;
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
if(!result){
usleep(100000);
while(1){
result = BrickPiUpdateValues();
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DCOM)){
X = ((BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][0]&0x01)?-1:1)*(BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][1]);
Z = ((BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][2]&0x01)?-1:1)*(BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][3]);
Y = ((BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][4]&0x01)?-1:1)*(BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DCOM][5]);
angle = atan2(X,Y);
if(angle<0) angle += 2*PI;
angle *= 180/PI;
printf("X: %d Y: %d Z: %d H:%f \n", X, Y, Z,angle);
}
}
usleep(100000);
}
}
return 0;
}
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
/* result = BrickPiChangeAddress(0, 2); // For changing the address of one of the slave uCs
printf("BrickPiChangeAddress: %d\n", result);
return 0;*/
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType[PORT_1] = TYPE_SENSOR_RAW;//TYPE_SENSOR_LIGHT_ON;
BrickPi.SensorType[PORT_2] = TYPE_SENSOR_RAW;//TYPE_SENSOR_LIGHT_ON;
BrickPi.SensorType[PORT_3] = TYPE_SENSOR_RAW;//TYPE_SENSOR_COLOR_FULL;
BrickPi.SensorType[PORT_4] = TYPE_SENSOR_RAW;//TYPE_SENSOR_ULTRASONIC_CONT;
BrickPi.SensorType [I2C_PORT] = TYPE_SENSOR_I2C_9V;
BrickPi.SensorI2CSpeed [I2C_PORT] = I2C_SPEED;
BrickPi.SensorI2CDevices [I2C_PORT] = 2;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 0;
BrickPi.SensorI2CAddr [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 0xA4;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_PSP_NX] = 0;
BrickPi.SensorI2CAddr [I2C_PORT][I2C_DEVICE_PSP_NX] = 0x02;
if(BrickPiSetupSensors())
return 0;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 2;
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 0;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_NXTCHUCK][0] = 0xF0;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_NXTCHUCK][1] = 0x55;
if(BrickPiUpdateValues())
return 0;
if(!(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_NXTCHUCK)))
return 0;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_NXTCHUCK][0] = 0xFB;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_NXTCHUCK][1] = 0x00;
if(BrickPiUpdateValues())
return 0;
if(!(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_NXTCHUCK)))
return 0;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_NXTCHUCK] = BIT_I2C_SAME;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 1;
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_NXTCHUCK] = 6;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_NXTCHUCK][0] = 0x00;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_PSP_NX] = 0;
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_PSP_NX] = 0;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_PSP_NX] = BIT_I2C_SAME;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_PSP_NX] = 1;
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_PSP_NX] = 6;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_PSP_NX][0] = 0x42;
BrickPi.SensorType [US_I2C_PORT] = US_I2C_TYPE;
BrickPi.SensorI2CSpeed [US_I2C_PORT] = US_I2C_SPEED;
BrickPi.SensorI2CDevices [US_I2C_PORT] = 1;
BrickPi.SensorSettings [US_I2C_PORT][US_I2C_DEVICE] = US_I2C_SETTINGS;
BrickPi.SensorI2CAddr [US_I2C_PORT][US_I2C_DEVICE] = 0x02;
BrickPi.SensorI2CWrite [US_I2C_PORT][US_I2C_DEVICE] = 1;
BrickPi.SensorI2CRead [US_I2C_PORT][US_I2C_DEVICE] = 1;
BrickPi.SensorI2COut [US_I2C_PORT][US_I2C_DEVICE][0] = 0x42;
BrickPi.SensorType[US_PORT] = TYPE_SENSOR_ULTRASONIC_CONT;
/* BrickPi.SensorType[PORT_2] = TYPE_SENSOR_RAW;//TYPE_SENSOR_LIGHT_ON;
BrickPi.SensorType[PORT_3] = TYPE_SENSOR_RAW;//TYPE_SENSOR_COLOR_FULL;
BrickPi.SensorType[PORT_4] = TYPE_SENSOR_RAW;//TYPE_SENSOR_ULTRASONIC_CONT;*/
BrickPi.MotorEnable[PORT_A] = 1;
BrickPi.MotorEnable[PORT_B] = 1;
BrickPi.MotorEnable[PORT_C] = 1;
// BrickPi.MotorEnable[PORT_D] = 0;
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
if(!result){
usleep(10000);
while(1){
result = BrickPiUpdateValues();
// printf("BrickPiUpdateValues: %d\n", result);
if(!result){
/* SpeedLeft = ((SY - 128) + (SX - 128)) * 2;
SpeedRight = ((SY - 128) - (SX - 128)) * 2;
if(SpeedLeft < -255)
SpeedLeft = -255;
else if(SpeedLeft > 255)
SpeedLeft = 255;
if(SpeedRight < -255)
SpeedRight = -255;
else if(SpeedRight > 255)
SpeedRight = 255;
printf("SX: %3.1d SY: %3.1d AX: %4.1d AY: %4.1d AZ: %4.1d B: %.1d L: %3.1d R: %3.1d\n", SX, SY, AX, AY, AZ, B, SpeedLeft, SpeedRight);*/
printf("Results: %3.1d %.1d %.1d\n", BrickPi.Sensor[US_PORT], BrickPi.Sensor[US_I2C_PORT], BrickPi.Sensor[I2C_PORT]);
if(BrickPi.Sensor[US_I2C_PORT] & (0x01 << US_I2C_DEVICE)){
printf("%3.1d\n", BrickPi.SensorI2CIn[US_I2C_PORT][US_I2C_DEVICE][0]);
}
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_PSP_NX)){
printf("%3.1d %3.1d %3.1d %3.1d %3.1d %3.1d\n", BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][0], BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][1], BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][2], BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][3], BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][4], BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_PSP_NX][5]);
}
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_NXTCHUCK)){
SX = BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][0];
SY = BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][1];
AX = (BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][2] << 2);
AY = (BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][3] << 2);
AZ = (BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][4] << 2);
B = ((~BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_NXTCHUCK][5]) & 0x03);
VZ = 0;
if(B & 0x01 && AX < 462)
VZ = (AX - 462);
else if(B & 0x01 && AX > 562)
VZ = (AX - 562);
if(VZ > 128)
VZ = 128;
if(VZ < -128)
VZ = -128;
VY = 0;
if(B & 0x01 && AY < 500)
VY = (AY - 500);
else if(B & 0x01 && AY > 562)
VY = (AY - 562);
if(VY > 128)
VY = 128;
if(VY < -128)
VY = -128;
VX = 0;
if(B & 0x01)
VX = (SX - 128);
VX *= 2;
VY = ((-VY) * 2);
VZ *= 2;
V1 = VX + VZ; //VX + VZ; // Vector Calculation for MotorA(V1)'s Power
V2 = (-VX / 2 - sqrt(3)/2 * VY) + VZ; //((-VX / 3) - (VY * 2 / 3)) + VZ //(-VX / 2 - math.sqrt(3)/2 * VY) + VZ; // Vector Calculation for MotorB(V2)'s Power
V3 = (-VX / 2 + sqrt(3)/2 * VY) + VZ; //((-VX / 3) + (VY * 2 / 3)) + VZ //(-VX / 2 + math.sqrt(3)/2 * VY) + VZ; // Vector Calculation for MotorC(V3)'s Power
if (V1 < 10 && V1 > 0)
V1 = 10;
if (V1 > -10 && V1 < 0)
V1 = -10;
if (V2 < 10 && V2 > 0)
V2 = 10;
if (V2 > -10 && V2 < 0)
V2 = -10;
if (V3 < 10 && V3 > 0)
V3 = 10;
if (V3 > -10 && V3 < 0)
V3 = -10;
printf("SX: %3.1d SY: %3.1d AX: %4.1d AY: %4.1d AZ: %4.1d B: %.1d\n", SX, SY, AX, AY, AZ, B);
printf("VX: %3.1d VY: %3.1d VZ: %3.1d V1: %3.1d V2: %3.1d V3: %3.1d\n", VX, VY, VZ, V1, V2, V3);
BrickPi.MotorEnable[PORT_A] = 1;
BrickPi.MotorEnable[PORT_B] = 1;
BrickPi.MotorEnable[PORT_C] = 1;
BrickPi.MotorSpeed[PORT_A] = V1;
BrickPi.MotorSpeed[PORT_B] = V2;
BrickPi.MotorSpeed[PORT_C] = V3;
}
else{
BrickPi.MotorEnable[PORT_A] = 0;
BrickPi.MotorEnable[PORT_B] = 0;
BrickPi.MotorEnable[PORT_C] = 0;
}
/*BrickPi.MotorEnable[PORT_A] = 0;
BrickPi.MotorEnable[PORT_B] = 1;
BrickPi.MotorEnable[PORT_C] = 1;
BrickPi.MotorEnable[PORT_D] = 1;
BrickPi.MotorSpeed[PORT_D] = BrickPi.Encoder[PORT_A];
BrickPi.MotorSpeed[PORT_B] = BrickPi.Encoder[PORT_A];
BrickPi.MotorSpeed[PORT_C] = BrickPi.Encoder[PORT_A];
printf("Encoder A: %6.1d\n", BrickPi.Encoder[PORT_A]);*/
}
usleep(10000);
}
}
return 0;
}
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType [I2C_PORT] = TYPE_SENSOR_I2C;
BrickPi.SensorI2CSpeed [I2C_PORT] = I2C_SPEED;
BrickPi.SensorI2CDevices [I2C_PORT] = 1; //number of devices in the bus
//writing 0x03 at address 0x3C to move the register to the first byte of data(0x03)
//and then reading 6 bytes from 0x3D*/
BrickPi.SensorI2CAddr [I2C_PORT][I2C_DEVICE_DTIR] = TIR_I2C_ADDR; //address for writing
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DTIR] = BIT_I2C_SAME;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_DTIR] = 1; //number of bytes to write
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DTIR] = 2; //number of bytes to read
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
if(!result){
usleep(100000);
while(1){
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DTIR][0] = TIR_OBJECT; //first measure object temperature
BrickPiSetupSensors(); //setup sensor for obj
result = BrickPiUpdateValues(); //write & read I2C and get values updates
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DTIR)){
t1= BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][0];
t2=BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][1];
temp = (float)((BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][1]<<8)+BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][0]);
temp=temp*0.02-0.01;
temp-=273.15;
}
}
usleep(50000); //giving some delay before acquiring ambient temp
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DTIR][0] = TIR_AMBIENT; //measure ambient temperature
BrickPiSetupSensors(); //setup sensor for amb
result = BrickPiUpdateValues(); //write & read I2C and get values updates
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DTIR)){
t1= BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][0];
t2=BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][1];
amb = (float)((BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][1]<<8)+BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DTIR][0]);
amb=amb*0.02-0.01;
amb-=273.15;
printf("Ambient Temp: %f Object Temp: %f\n",amb, temp);
}
}
usleep(300000); //delay to avoid RPi crash
}
}
return 0;
}
// 打印机初始化
void Printer_Init(void) {
int i;
// 获取打印速度参数
PrintSpeed= 10 + ( (((Read_ADC0(2)>0xF00)?1:0)<<2) |( ((Read_ADC0(1)>0xF00)?1:0)<<1) | ((Read_ADC0(0)>0xF00)?1:0) ); //打印速度+SW12加热时间
// 字体格式
FontType = 1; // 24X24字体
AlignType = 0; // 左对齐
OldAlignType = 0;
LineDiffRate = 2; // 两行间隙为1/2个字符
// 打印数据变量初始化
WritePrintBufferIndex = 0;
ReadPrintListIndex = WritePrintListIndex = 0;
PrintNodeList[WritePrintListIndex].Begin = WritePrintBufferIndex;
PrintNodeList[WritePrintListIndex].End = WritePrintBufferIndex;
PrintNodeList[WritePrintListIndex].Align = AlignType;
PrintNodeList[WritePrintListIndex].Font = FontType;
PrintNodeList[WritePrintListIndex].LDRate = LineDiffRate;
PrintNodeList[WritePrintListIndex].Cut = 0;
PrintColIndex = 0xFFFF;
// 打印缓冲节点
ReadPrintLineIdx = WritePrintLineIdx = 0;
memset (PrintLines, 0, sizeof(PrintLines));
// 空行
memset (Empty, 0, sizeof(Empty));
SetAutoLR(1);
// 锁的检测
OpenDoorTick[0] = OpenDoorTick[1] = 0;
CloseDoorTick[0] = CloseDoorTick[1] = 0;
// 开始打印时钟
StartTick = 0;
// 发送两字节过去给IC板
SendIC (0x18);
SendIC (0x19);
// 进点纸
MOTOR_Open; //打印机初始化
Power_Off; //关打印数据未锁定
for (i = 0; i < MAX_POINT; i++) {
PrntCLK = 0;
PrntDAT = 0;
PrntCLK = 1;
}//清热敏片
ClearTick(); // 清时间
D3Err = 0; // 清切纸电机报错
// 切纸电机未到位
if (CUTTER_RESETED) {
CUTTER_HIGH = 1;
CUTTER_LOW = 0; // 切刀转动
while(CUTTER_RESETED && GetTick() < 50000);//限5S (0.1ms*50000)
CUTTER_HIGH = 0;
CUTTER_LOW = 0; // 切刀停止
if (GetTick() >= 30000) {
D3Err = 1;
LED3_ON;
} //报错
} RollPaper(1, 200);
}
int main() {
ClearTick();
I2C_file_descriptor = open("/dev/i2c-1", O_RDWR);
printf("I2C open: %d\n", I2C_file_descriptor);
if(I2C_file_descriptor == -1)
return 0;
result = NXTChuckInit(I2C_file_descriptor);
printf("NXTChuckInit: %d\n", result);
if(result != NXTChuck_COM_SUCCESS)
return 0;
/* while(1){
result = NXTChuckReadNunchuk(I2C_file_descriptor, &SX, &SY, &AX, &AY, &AZ, &B);
if(result != NXTChuck_COM_SUCCESS){
printf("NXTChuckReadNunchuk: %d\n", result);
}
else{
SpeedLeft = ((SY - 128) + (SX - 128)) * 2;
SpeedRight = ((SY - 128) - (SX - 128)) * 2;
if(SpeedLeft < -255){
SpeedLeft = -255;
}
else if(SpeedLeft > 255){
SpeedLeft = 255;
}
if(SpeedRight < -255){
SpeedRight = -255;
}
else if(SpeedRight > 255){
SpeedRight = 255;
}
printf("SX: %3.1d SY: %3.1d AX: %4.1d AY: %4.1d AZ: %4.1d B: %.1d L: %3.1d R: %3.1d\n", SX, SY, AX, AY, AZ, B, SpeedLeft, SpeedRight);
// usleep(10000);
// ioctl(I2C_file_descriptor, I2C_SLAVE, 0x20);
// I2CArray[0] = SX;
// write(I2C_file_descriptor, I2CArray, 1);
}
usleep(10000);
}*/
result = BrickPiSetup();
printf("BrickPiSetup: %d\n", result);
if(result)
return 0;
/* result = BrickPiChangeAddress(0, 2); // For changing the address of one of the slave uCs
printf("BrickPiChangeAddress: %d\n", result);
return 0;*/
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType[PORT_1] = TYPE_SENSOR_RAW;
BrickPi.SensorType[PORT_2] = TYPE_SENSOR_RAW;//TYPE_SENSOR_LIGHT_ON;
BrickPi.SensorType[PORT_3] = TYPE_SENSOR_RAW;//TYPE_SENSOR_COLOR_FULL;
BrickPi.SensorType[PORT_4] = TYPE_SENSOR_RAW;//TYPE_SENSOR_ULTRASONIC_CONT;
BrickPi.MotorEnable[PORT_A] = 1;
BrickPi.MotorEnable[PORT_B] = 1;
BrickPi.MotorEnable[PORT_C] = 1;
BrickPi.MotorEnable[PORT_D] = 0;
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %d\n", result);
if(!result){
while(1){
result = BrickPiUpdateValues();
// printf("BrickPiUpdateValues: %d\n", result);
if(!result){
result = NXTChuckReadNunchuk(I2C_file_descriptor, &SX, &SY, &AX, &AY, &AZ, &B);
if(result != NXTChuck_COM_SUCCESS){
printf("NXTChuckReadNunchuk: %d\n", result);
}
else{
/* SpeedLeft = ((SY - 128) + (SX - 128)) * 2;
SpeedRight = ((SY - 128) - (SX - 128)) * 2;
if(SpeedLeft < -255)
SpeedLeft = -255;
else if(SpeedLeft > 255)
SpeedLeft = 255;
if(SpeedRight < -255)
SpeedRight = -255;
else if(SpeedRight > 255)
SpeedRight = 255;
printf("SX: %3.1d SY: %3.1d AX: %4.1d AY: %4.1d AZ: %4.1d B: %.1d L: %3.1d R: %3.1d\n", SX, SY, AX, AY, AZ, B, SpeedLeft, SpeedRight);*/
VZ = 0;
if(B & NXTChuck_N_BTN_Z && AX < 462)
VZ = (AX - 462);
else if(B & NXTChuck_N_BTN_Z && AX > 562)
VZ = (AX - 562);
if(VZ > 128)
VZ = 128;
if(VZ < -128)
VZ = -128;
VY = 0;
if(B & NXTChuck_N_BTN_Z && AY < 500)
VY = (AY - 500);
else if(B & NXTChuck_N_BTN_Z && AY > 562)
VY = (AY - 562);
if(VY > 128)
VY = 128;
if(VY < -128)
VY = -128;
VX = 0;
if(B & NXTChuck_N_BTN_Z)
VX = (SX - 128);
VX *= 2;
VY = ((-VY) * 2);
VZ *= 2;
V1 = VX + VZ; //VX + VZ; // Vector Calculation for MotorA(V1)'s Power
V2 = (-VX / 2 - sqrt(3)/2 * VY) + VZ;//((-VX / 3) - (VY * 2 / 3)) + VZ //(-VX / 2 - math.sqrt(3)/2 * VY) + VZ; // Vector Calculation for MotorB(V2)'s Power
V3 = (-VX / 2 + sqrt(3)/2 * VY) + VZ;//((-VX / 3) + (VY * 2 / 3)) + VZ //(-VX / 2 + math.sqrt(3)/2 * VY) + VZ; // Vector Calculation for MotorC(V3)'s Power
if (V1 < 10 && V1 > 0)
V1 = 10;
if (V1 > -10 && V1 < 0)
V1 = -10;
if (V2 < 10 && V2 > 0)
V2 = 10;
if (V2 > -10 && V2 < 0)
V2 = -10;
if (V3 < 10 && V3 > 0)
V3 = 10;
if (V3 > -10 && V3 < 0)
V3 = -10;
printf("SX: %3.1d SY: %3.1d AX: %4.1d AY: %4.1d AZ: %4.1d B: %.1d\n", SX, SY, AX, AY, AZ, B);
printf("VX: %3.1d VY: %3.1d VZ: %3.1d V1: %3.1d V2: %3.1d V3: %3.1d\n", VX, VY, VZ, V1, V2, V3);
BrickPi.MotorSpeed[PORT_A] = V1;
BrickPi.MotorSpeed[PORT_B] = V2;
BrickPi.MotorSpeed[PORT_C] = V3;
//BrickPi.MotorSpeed[PORT_D] = SpeedLeft;
}
// BrickPi.MotorSpeed[PORT_A] = BrickPi.Encoder[PORT_B] / 8;
// BrickPi.MotorSpeed[PORT_C] = -BrickPi.Encoder[PORT_B];
// printf("S1: %3.1d %3.1d %3.1d %3.1d %3.1d ", BrickPi.Sensor[PORT_1], BrickPi.SensorArray[PORT_1][INDEX_BLANK], BrickPi.SensorArray[PORT_1][INDEX_RED], BrickPi.SensorArray[PORT_1][INDEX_GREEN], BrickPi.SensorArray[PORT_1][INDEX_BLUE]);
// printf("S2: %3.1d %3.1d %3.1d %3.1d %3.1d\n", BrickPi.Sensor[PORT_2], BrickPi.SensorArray[PORT_2][INDEX_BLANK], BrickPi.SensorArray[PORT_2][INDEX_RED], BrickPi.SensorArray[PORT_2][INDEX_GREEN], BrickPi.SensorArray[PORT_2][INDEX_BLUE]);
//// printf("S3: %3.1d %3.1d %3.1d %3.1d %3.1d ", BrickPi.Sensor[PORT_3], BrickPi.SensorArray[PORT_3][INDEX_BLANK], BrickPi.SensorArray[PORT_3][INDEX_RED], BrickPi.SensorArray[PORT_3][INDEX_GREEN], BrickPi.SensorArray[PORT_3][INDEX_BLUE]);
// printf("S4: %3.1d %3.1d %3.1d %3.1d %3.1d\n", BrickPi.Sensor[PORT_4], BrickPi.SensorArray[PORT_4][INDEX_BLANK], BrickPi.SensorArray[PORT_4][INDEX_RED], BrickPi.SensorArray[PORT_4][INDEX_GREEN], BrickPi.SensorArray[PORT_4][INDEX_BLUE]);
// printf("Sensor 2.2: %d\n", BrickPi.Sensor2[PORT_2]);
//// printf("S1: %3.1d ", BrickPi.Sensor[PORT_1]);
// printf("S2: %3.1d ", BrickPi.Sensor[PORT_2]);
//printf("Sensor 3: %d\n", BrickPi.Sensor[PORT_3]);
//// printf("S4: %3.1d\n", BrickPi.Sensor[PORT_4]);
//// printf("Encoder A: %6.1d ", BrickPi.Encoder[PORT_A]);
//// printf("Encoder B: %6.1d\n", BrickPi.Encoder[PORT_B]);
//printf("Encoder C: %d\n", BrickPi.Encoder[PORT_C]);
//printf("Encoder D: %d\n", BrickPi.Encoder[PORT_D]);
// printf("%3.1d ", BrickPi.Sensor[PORT_4]);
}
usleep(10000);
}
}
return 0;
}
