void motorDemo(void) {
UARTprintf("Press:\n w-forward\n s-backward\n a-left\n ");
UARTprintf("d-right\n space-stop\n enter-quit\n");
{
// values should range between -128 and 127?
signed char left = 0, right = 0, maxSpeed = 127;
char newline = 13;
char ch = getc();
while(ch != newline) {
ch = getc();
putc(ch);
if (ch == 'w') {
left = maxSpeed;
right = -maxSpeed;
} else if (ch == 's') {
left = -maxSpeed;
right = maxSpeed;
} else if (ch == 'a') {
left = -maxSpeed;
right = -maxSpeed;
} else if (ch == 'd') {
left = maxSpeed;
right = maxSpeed;
} else if (ch == ' ') {
left = 0;
right = 0;
}
SetMotorPowers(left, right);
}
}
SetMotorPowers(0,0);
UARTprintf("\n");
}
void rotate_time(int degree) {
SetMotorPowers(80,-80);
WaitUS(6874*degree);
SetMotorPowers(0,0);
angle += degree;
return;
}
int follow_wall(bool(*cond)(void)) {
unsigned long ADCValue = getADCValue();
int speed = 127;
if((*cond)()) return COND_BREAK;
if (wallApproach()) return CORNER;
if(ADCValue > 500)
{
if(ADCValue > 600)
{
SetMotorPowers (-speed, speed);
}
else
SetMotorPowers(speed/3, speed);
UARTprintf("IR value1: %d\r",ADCValue);
}
//adj = (600 - ADCValue)/10;
//rotate (adj);
else if(ADCValue < 300)
{
if(ADCValue < 150)
{
SetMotorPowers(speed, -50);
}
else
SetMotorPowers(speed, speed/3);
UARTprintf("\nIR value2: %d\r",ADCValue);
}
return RETURN_W;
}
void BackOut(void){
SetMotorPowers(-40,-128);
Wait(500);
SetMotorPowers(32,127);
Wait(500);
SetMotorPowers(127,127);
Wait(300);
}
void rotate_enc(int degree) {
int ldiff = 0, rdiff = 0;
long l = 0, r = 0, count = 0;//oldl = 0, oldr = 0,
long break_val = (BOT_WIDTH * ABS(degree)) * (epr/(WHEEL_RAD*180));
PresetEncoderCounts(0,0);
if (degree > 0) SetMotorPowers(80,-80);
else SetMotorPowers(-80,80);
while (count * 4 < break_val)
{
GetEncoderCounts(&l, &r);
ldiff = (l>r)?0:r-l;
rdiff = (r>l)?0:l-r;
SetMotorPowers(127-ldiff,-128+rdiff);
count = (ABS(l)+ABS(r))/2;
UARTprintf("Count: %10d %10d\r",count,break_val);
//oldl = l; oldr = r;
WaitUS(1000);
}
}
void move_enc(int distance) {
long enc0 = 0, enc1 = 0;
long oldr = 0, oldl = 0, count = 0;
long break_val = distance * epr /WHEEL_CIRC;
PresetEncoderCounts(0,0);
SetMotorPowers(127,127);
while(count < break_val)
{
GetEncoderCounts(&enc0, &enc1);
pid((enc0-oldr),(enc1-oldl));
oldr = enc0;
oldl = enc1;
count = (enc0 + enc1)/2;
//UARTprintf("Count: %10d %10d\r",WHEEL_CIRC*count,distance*epr);
//WaitUS(100);
}
SetMotorPowers(0,0);
return;
}
void SetMotorPowersBlocking(int left, int right){
if(left < -128)
left = -128;
if(left > 127)
left = 127;
if(right < -128)
right = -128;
if(right > 127)
right = 127;
SetMotorPowers( left*LEFT_MOTOR_OFFSET/LEFT_MOTOR_OFFSET_DIV , right );
}
/* ******************************************************************** */
void SetMotors_Calibrated ( int power0, int power1 )
{
/* First, check for a stop */
if ((power0 == STOP) && (power1 == STOP)) {
SetMotorPowers(0,0);
}
/* Bound checking -- make sure valid powers sent */
else if (power0 > 127) {
UARTprintf("\nError -- motor0 power too high\n");
SetMotorPowers(0,0);
}
else if (power0 < -128) {
UARTprintf("\nError -- motor0 power too low\n");
SetMotorPowers(0,0);
}
else if (power1 > 127) {
UARTprintf("\nError -- motor1 power too high\n");
SetMotorPowers(0,0);
}
else if (power1 < -128) {
UARTprintf("\nError -- motor1 power too low\n");
SetMotorPowers(0,0);
}
/* Call RASLib function with correction */
else {
SetMotorPowers(power0 + LEFT_MOTOR_CORRECTION,
power1 + RIGHT_MOTOR_CORRECTION);
}
}
/* ******************************************************************** */
void CalibrateMotors ( void )
{
int leftSpeed = HALF_POWER, rightSpeed = HALF_POWER;
char ch;
/* Display the UI instructions */
UARTprintf("Setting motors to half-speed. Adjust left motor\n");
UARTprintf("Press: a-turn left more, f-turn right more\n");
UARTprintf("Press SPACE to start/stop.\n");
/* Responds to input */
while (ch = getc()) {
/* Display current values */
UARTprintf("Left Speed: %d\n", leftSpeed);
UARTprintf("Right Speed: %d\n", rightSpeed);
/* Choose case */
switch (ch) {
case 'a':
leftSpeed -= 1;
break;
case 'f':
leftSpeed += 1;
break;
case ' ':
SetMotorPowers(0,0);
while (getc() != ' '); /* do nothing */
UARTprintf("\n");
break;
}
SetMotorPowers(leftSpeed, rightSpeed);
}
}
void arc(int radius, int degree) {
long l = 0, r = 0, avg = 0;//, ol = 0, ort = 0;
long long break_val = 0;
PresetEncoderCounts(0,0);
break_val = (epr * ABS(radius));
UARTprintf("Rad: %3d %d\n",ABS(radius),break_val);
//if (ABS(radius) < BOT_WIDTH/2)
// return;
while(break_val > WHEEL_RAD * avg * 180 * degree)
{
GetEncoderCounts(&l,&r);
//pid(radius+BOT_WIDTH,radius-BOT_WIDTH);
//pid(radius,BOT_WIDTH);
SetMotorPowers(MEDIAN(0,127,127 * (radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2)),
MEDIAN(0,127,127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2)));
avg = (l+r)/2;
UARTprintf("Compare: %10d %10d Powers2: %5d %5d\r",break_val, WHEEL_RAD * avg * degree,
127*(radius + BOT_WIDTH/2) / (radius - BOT_WIDTH/2),127*(radius - BOT_WIDTH/2) / (radius + BOT_WIDTH/2));
//UARTprintf("Average: %10d %10d\r",(radius + BOT_WIDTH>>1),(radius - BOT_WIDTH>>1));
//ol = l; ort = r;
}
SetMotorPowers(0,0);
}
int main(void){
char i;
unsigned char data[16];
short wiichuck[7], xinit=0, yinit=0, l_vel, r_vel;
int xpow, ypow;
LockoutProtection();
InitializeMCU();
InitializeUART();
InitializeI2C();
InitializeServos();
SetServoPosition(SERVO_0, 140);
InitializeMotors(true, false);
InitializeEncoders(true, false);
// UARTprintf("Initializing Nunchuck\n\n");
// I2CSend(0x52<<1, 2, 0x40, 0x00);
// Wait(25);
init_nunchuck();
// Wireless Nunchucks Zero @ 128
xinit = yinit = 128;
while(1){
//Start Recalculating Values
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
Wait(1);
I2CSend(0x52<<1, 1, 0x00);
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
default:
UARTprintf("WTF: %d\n", I2CMasterErr(I2C0_MASTER_BASE));
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
Wait(1);
I2CRecieve(0x52<<1, data, 6); // Nunchuck data is 6 bytes, but for whatever reason, MEMOREX Wireless Nunchuck wants to send 8...
if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){
UARTprintf("Send Zero Error:\n");
switch(I2CMasterErr(I2C0_MASTER_BASE)){
case I2C_MASTER_ERR_ADDR_ACK:
UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n");
break;
case I2C_MASTER_ERR_DATA_ACK:
UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n");
break;
case I2C_MASTER_ERR_ARB_LOST:
UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n");
break;
}
// Reinitialize Nunchuck on error
init_nunchuck();
}else{
//for(i=0; i<6; i++)
// data[i] = (data[i] ^ 0x17) + 0x17; // Nintendo decided to encrypt thir data...
// Save Joystick Data
wiichuck[0] = data[1]; // X Axis Joystick
wiichuck[1] = data[0]; // Y Axis Joystick
wiichuck[2] = (((unsigned short) data[2]) << 2) + (((unsigned short) data[5]) & (3<<2)); // X Axis Accel
wiichuck[3] = (((unsigned short) data[3]) << 2) + (((unsigned short) data[5]) & (3<<4)); // Y Axis Accel
wiichuck[4] = (((unsigned short) data[4]) << 2) + (((unsigned short) data[5]) & (3<<6)); // Z Axis Accel
wiichuck[5] = data[5] & (1 << 1) ? 0 : 1; //'C' Button
wiichuck[6] = data[5] & (1 << 0) ? 0 : 1; //'Z' Button
//if (xinit == 0 && yinit == 0){
// xinit = wiichuck[0]-127;
// yinit = wiichuck[1]-127;
//}else{
xpow = (wiichuck[0]-xinit)/2;
ypow = (wiichuck[1]-yinit)/2;
l_vel = (xpow - ypow)*2;
r_vel = (xpow + ypow)*2;
l_vel = l_vel > 127 ? 127 : l_vel;
r_vel = r_vel > 127 ? 127 : r_vel;
l_vel = l_vel < -127 ? -127 : l_vel;
r_vel = r_vel < -127 ? -127 : r_vel;
//UARTprintf("X: %d\tY: %d\n", xpow*2, ypow*2);
SetMotorPowers(l_vel / (wiichuck[5]==0 ? 2 : 1), r_vel / (wiichuck[5]==0 ? 2 : 1));
UARTprintf("Motor L: %d\tMotor R: %d\n", l_vel, r_vel);
SetServoPosition(SERVO_0, wiichuck[6]==1 ? 255 : 140);
UARTprintf("Nunchuck Data:\n");
for(i=0; i<7; i++){
UARTprintf(" %d\n", wiichuck[i]);
}NL;
Wait(100);
}
}
}
}
void move_time(int distance) {
SetMotorPowers(-110,-127);
WaitUS(distance*500);
SetMotorPowers(0,0);
}
void pid(signed long e0, signed long e1) {
int error = (e1-e0);
SetMotorPowers(MEDIAN(-80,80,64-error/4),
MEDIAN(-80,80,64+error/4));
}
void goForwardBlocking(void) {
motor_L = 85;
motor_R = 120;
SetMotorPowers(motor_L, motor_R);
piderror = 0;
}
/* ******************************************************
main program of wall following
IR.leftIRSensorValue = value read by left IR Sensor
IR.rightIRSensorValue = value read by right IR Sensor
IR.frontIRSensorValue = value read by front IR Sensor
****************************************************** */
void FollowWall (void){
/*distance to follow is good = , and distance to check before turning is far = */
unsigned long good = 700, far = 400;
// unsigned long maximum = 200, minimum = 900;
char direction;
signed char left = 0, right = 0, maxSpeed = 127;
while(1){
UARTprintf("left IR value: %d\n\r",IR.left);
UARTprintf("right IR value: %d\n\r",IR.right);
UARTprintf("front IR value: %d\n\r",IR.front);
/* if go far away from left wall turn left to approach the wall
left and front designated value, right 2 times larger than the designated value*/
if (IR.left >= good && IR.right >= far && IR.front >= good ){
direction = 'l';
left = -maxSpeed;
right = -maxSpeed;
}
/* if go far away from right wall turn right to approach the wall
right and front designated value, leftt 2 times larger than the designated value*/
else if (IR.left >= far && IR.right >= good && IR.front >= good ){
direction = 'r';
left = maxSpeed;
right = maxSpeed;
}
/* if go too close to the right wall turn left, away from the wall */
else if (IR.left >= good && IR.right <= good && IR.front >= good ){
direction = 'l';
left = -maxSpeed;
right = -maxSpeed;
}
/* if go too close to the left wall turn right, away from the wall */
else if (IR.left <= good && IR.right >= good && IR.front >= good ){
direction = 'r';
left = maxSpeed;
right = maxSpeed;
}
/* if go too close to the front wall when following the wall on the left, turn right*/
else if ((IR.left <= far || IR.right >= far )&& (IR.front <= far && IR.front >= good)){
direction = 'r';
left = maxSpeed;
right = maxSpeed;
}
/* if go too close to the front wall when following the wall on the right, turn left*/
else if ((IR.left >= far || IR.right <= far )&& IR.front <= far && IR.front >= good ){
direction = 'l';
left = -maxSpeed;
right = -maxSpeed;
}
/* if there is a wall in front and both sides of the robot is also near to the wall, reverse */
else if (((IR.left <= far && IR.left >= good )||(IR.right <= far && IR.right >= good ))&& (IR.front >= good && IR.front <= far)){
direction = 'b';
left = -maxSpeed;
right = maxSpeed;
}
/* if the value of the left and right IR is between the range, stay on the course*/
else if ((IR.left >= good && IR.left <= far )||(IR.right >= good &&IR.right <= far )&& IR.front >= far ){
direction = 'f';
left = maxSpeed;
right = -maxSpeed;
}
SetMotorPowers(left, right);
UARTprintf("robot going: %c\n\r" ,direction);
}
}
