/*
* stop()
* @function : Stop drone
* @detail : 공통부분. 속도 0 지정
*/
void Motor::stop()
{
softPwmWrite(pin, 0);
}
int setServo (int no, int val)
{
softPwmWrite(no, val);
return 0;
}
void L293d::stopMotor() {
softPwmWrite(motor.pin1, 0);
softPwmWrite(motor.pin2, 0);
running = false;
}
int ps3c_test(struct ps3ctls *ps3dat) {
unsigned char i;
unsigned char nr_btn = ps3dat->nr_buttons;
unsigned char nr_stk = ps3dat->nr_sticks;
int xx,yy,x,y,z;
setPCA9685Duty(fds , 0 , 0 , 276 + s0 / 2);
setPCA9685Duty(fds , 1 , 0 , 276 + s1 / 2);
// printf("%d %d\n",nr_btn,nr_stk);
printf(" 1=%2d ",ps3dat->button[PAD_KEY_LEFT]);
printf(" 2=%2d ",ps3dat->button[PAD_KEY_RIGHT]);
printf(" 3=%2d ",ps3dat->button[PAD_KEY_UP]);
printf(" 4=%2d ",ps3dat->button[PAD_KEY_DOWN]);
printf(" 5=%4d ",ps3dat->stick [PAD_LEFT_X]);
printf(" 6=%4d ",ps3dat->stick [PAD_LEFT_Y]);
printf(" 7=%4d ",ps3dat->stick [PAD_RIGHT_X]);
printf(" 8=%4d ",ps3dat->stick [PAD_RIGHT_Y]);
if (ps3dat->button[PAD_KEY_LEFT]) { s0++; if(s0 > +200) s0 = +200; };
if (ps3dat->button[PAD_KEY_RIGHT]) { s0--; if(s0 < -200) s0 = -200; };
if (ps3dat->button[PAD_KEY_UP]) { s1++; if(s1 > +200) s1 = +200; };
if (ps3dat->button[PAD_KEY_DOWN]) { s1--; if(s1 < -200) s1 = -200; };
printf(" s0=%4d ",s0);
printf(" s1=%4d ",s1);
printf("\n");
xx = ps3dat->stick [PAD_LEFT_X];
yy = ps3dat->stick [PAD_LEFT_Y];
z = ps3dat->stick [PAD_RIGHT_Y];
x = xx * cos(-M_PI/4) - yy * sin(-M_PI/4);
y = xx * sin(-M_PI/4) + yy * cos(-M_PI/4);
if(abs(z) < 5) {
softPwmWrite( 5,0);
softPwmWrite( 6,0);
softPwmWrite(26,0);
softPwmWrite(27,0);
softPwmWrite(28,0);
softPwmWrite(29,0);
} else if(z > 0) {
softPwmWrite( 5,abs(z));
softPwmWrite( 6,0);
softPwmWrite(26,abs(z));
softPwmWrite(27,0);
softPwmWrite(28,abs(z));
softPwmWrite(29,0);
} else {
softPwmWrite( 5,0);
softPwmWrite( 6,abs(z));
softPwmWrite(26,0);
softPwmWrite(27,abs(z));
softPwmWrite(28,0);
softPwmWrite(29,abs(z));
};
if(abs(x) < 5) {
softPwmWrite(14,0);
softPwmWrite(23,0);
} else if(x > 0) {
softPwmWrite(14,abs(x));
softPwmWrite(23,0);
} else {
softPwmWrite(14,0);
softPwmWrite(23,abs(x));
};
if(abs(y) < 5) {
softPwmWrite(24,0);
softPwmWrite(25,0);
} else if(y > 0) {
softPwmWrite(24,abs(y));
softPwmWrite(25,0);
} else {
softPwmWrite(24,0);
softPwmWrite(25,abs(y));
};
if(ps3dat->button[PAD_KEY_CROSS]==1) return -1; // end of program
return 0;
}
void servo_rotate(char posChar)
{
printf("GET IN SERVO: %c",posChar);
switch(posChar)
{
case 'u': //camera go up
i_count=0;
posHor++;
if(posHor>20)posHor=20;
softPwmWrite(1,175+posHor);
printf("(^_^) Camera is going up.\n");
break;
case 'g': //camera go down
i_count=0;
posHor--;
if(posHor<0)posHor=0;
softPwmWrite(1,175+posHor);
printf("(^_^) Camera is going down.\n");
break;
case 'l': //camera turn the lerf
i_count=0;
posVer--;
if(posVer<0)posVer=0;
softPwmWrite(0,175+posVer);
printf("(^_^) Camera is turning left.\n");
break;
case 'r': //camera turn the right
i_count=0;
posVer++;
if(posVer>20)posVer=20;
softPwmWrite(0,175+posVer);
printf("(^_^) Camera is turning right.\n");
break;
case '1':
//get temperature data
printf("(^_^) Send temperature data to server.\n");
serialPuts (fd, "#1") ;
break;
case '2':
// enable warning
printf("(^_^) Enable security warning.\n");
serialPuts (fd, "#2") ;
break;
case '3':
// disable warning
printf("(^_^) Disable security warning.\n");
serialPuts (fd, "#3") ;
break;
case '4':
// get status light 1 2 ...
printf("(^_^) Get status all Lights in your house.\n");
serialPuts (fd, "#4") ;
break;
case '5':
// turn on/off light 1
serialPuts (fd, "#5") ;
printf("\nTurn on light (1) : %d\n",fd);
break;
case '6':
// turn on/off light 2
serialPuts (fd, "#6") ;
printf("\nTurn off light (1) : %d\n",fd);
break;
case '7':
// turn on/off light 2
serialPuts (fd, "#7") ;
printf("\nTurn on light (2) : %d\n",fd);
break;
case '8':
// turn on/off light 2
serialPuts (fd, "#8") ;
printf("\nTurn off light (2) : %d\n",fd);
break;
case '9':
// turn on/off light 2
serialPuts (fd, "#9") ;
printf("\nTurn on light (3) : %d\n",fd);
break;
case 'a':
// turn on/off light 2
serialPuts (fd, "#a") ;
printf("\nTurn off light (3) : %d\n",fd);
break;
case 'b':
// turn on/off light 2
serialPuts (fd, "#b") ;
printf("\nTurn on light (4) : %d\n",fd);
break;
case 'c':
// turn on/off light 2
serialPuts (fd, "#c") ;
printf("\nTurn off light (4) : %d\n",fd);
break;
case 'd':
// turn on/off light 2
serialPuts (fd, "#d") ;
break;
case 'e':
// turn on/off light 2
serialPuts (fd, "#e") ;
break;
case 'f':
// Update all data
serialPuts (fd, "#f") ;
printf("\nUpdate data : %d\n",fd);
break;
}
}
void body_rgbcolor(SharedVariable* sv) {
if(sv->dead!=1){
if(sv->reset==1)
{
// softPwmWrite(PIN_RED, 0); //rgb=blue
// softPwmWrite(PIN_GREEN, 0);
// softPwmWrite(PIN_BLUE, 0);
softPwmWrite(PIN_RED, 0); //rgb=blue
softPwmWrite(PIN_GREEN, 0);
softPwmWrite(PIN_BLUE, 100);
}
else
{
if(sv->track_obj==1 && sv->reset==0)
{
// softPwmWrite(PIN_RED, 0);
// softPwmWrite(PIN_GREEN, 0);
// softPwmWrite(PIN_BLUE, 0);
softPwmWrite(PIN_RED, 46); //rgb=magenta
softPwmWrite(PIN_GREEN, 0);
softPwmWrite(PIN_BLUE, 93);
sv->dead=1;
// delay(1000);
}
else if(sv->shock_obj==1 && sv->reset==0)
{
// softPwmWrite(PIN_RED, 0);
// softPwmWrite(PIN_GREEN, 0);
// softPwmWrite(PIN_BLUE, 0);
softPwmWrite(PIN_RED, 100); //rgb=red
softPwmWrite(PIN_GREEN, 0);
softPwmWrite(PIN_BLUE, 0);
// printf("RGB=red\n");
sv->dead=1;
// delay(1000);
}
else
{
// softPwmWrite(PIN_RED, 0);
// softPwmWrite(PIN_GREEN, 0);
// softPwmWrite(PIN_BLUE, 0);
softPwmWrite(PIN_RED, 0); //rgb=green
softPwmWrite(PIN_GREEN, 100);
softPwmWrite(PIN_BLUE, 0);
// sv->reset=0;
// delay(1000);
}
}
}
}
void ledColorSet(uchar r_val, uchar g_val, uchar b_val)
{
softPwmWrite(LedPinRed, r_val);
softPwmWrite(LedPinGreen, g_val);
softPwmWrite(LedPinBlue, b_val);
}
void close()
{
softPwmWrite(pinCtrl, 0);
exit(0);
}
void DCMotor::stop()
{
softPwmWrite(motorA, 100);
softPwmWrite(motorB, 100);
}
void right(int speed) {
softPwmWrite(AIA_PIN, 0);
softPwmWrite(AIB_PIN, A_SCALE * speed);
softPwmWrite(BIA_PIN, B_SCALE * speed);
softPwmWrite(BIB_PIN, 0);
}
void forward(int speed) {
softPwmWrite(AIA_PIN, A_SCALE * speed);
softPwmWrite(AIB_PIN, 0);
softPwmWrite(BIA_PIN, B_SCALE * speed);
softPwmWrite(BIB_PIN, 0);
}
void backward(int speed) {
softPwmWrite(AIA_PIN, 0);
softPwmWrite(AIB_PIN, 0.8 * speed);
softPwmWrite(BIA_PIN, 0);
softPwmWrite(BIB_PIN, 1.0 * speed);
}
