void fahren2(void){
if (abstandvorne > 50)
{
back = 0;
}
ar = abstandrechts;
av = abstandvorne;
al = abstandlinks;
if ((abstandvorne < 30) || (back == 1))
{
back = 1;
fahr(-20);
if (ar > al)
{
servo(-10);
}
else
{
servo(10);
}
}
else
{
fahr(20);
servo(pReglerServoRechts(ar, al, av));
}
}
robot::robot() :
left_arm(servo(1, left_arm_min, left_arm_min + arm_range)),
right_arm(servo(0, right_arm_min, right_arm_min - arm_range)),
head(servo(2, head_center - head_range, head_center + head_range)),
torso(servo(3, torso_center - torso_range, torso_center + torso_range)),
tilt(servo(15, tilt_center - tilt_range, tilt_center + tilt_range)),
left_eye(rgb_led(4)),
right_eye(rgb_led(8)),
chest(led_grid(2, 3, 4, 5)),
voice(speaker(9)){ }
void initializeRobot()
{
servo(dump)=100;
servo(clamp)=0;
servo[score]=120;
forward_initizalize();
return;
}
void main()
{
lcd("Press SW1");
sw1_press();
while(1)
{
servo(0,20);
sleep(2000);
servo(0,60);
sleep(2000);
}
}
void main()
{
unsigned int dist,dist_l,dist_r;
servo(0,GET_MID);
lcd("Press SW1");
sw1_press();
while(1)
{
servo(0,GET_MID);
dist = srf05_dist(ECHO_PB4,PULSE_PB1);
if(dist>=3 && dist<=12)
{
pause();
servo(0,GET_LEFT);
sleep(2000);
dist_l = srf05_dist(ECHO_PB4,PULSE_PB1);
servo(0,GET_RIGHT);
sleep(2000);
dist_r = srf05_dist(ECHO_PB4,PULSE_PB1);
servo(0,GET_MID);
if(dist_l>dist_r && dist_l>DIST_MIN)
{
turn_left(200);
}
else if(dist_r>dist_l && dist_r>DIST_MIN)
{
turn_right(200);
}
else if(dist_r==dist_l && dist_r>DIST_MIN)
{
turn_right(200);
}
else
{
backward(1000);
turn_right(500);
}
}
else
{
forward(10);
}
}
}
int main(int argc, char **argv) {
if(argc < 5) {
std::cout << "Invalid number of args." << std::endl;
return 0;
}
auto maxPwm = atof(argv[1]);
if(maxPwm < 1.34 || maxPwm > 1.7) {
std::cout << "Max pulse width needs to be in range 1.34-1.7." << std::endl;
return 0;
}
auto pGain = atof(argv[2]);
if(pGain < 0) {
std::cout << "Proportional gain needs to be larget than zero." << std::endl;
return 0;
}
auto iGain = atof(argv[3]);
if(iGain < 0) {
std::cout << "Integral gain needs to be larget than zero." << std::endl;
return 0;
}
auto dGain = atof(argv[4]);
if(dGain < 0) {
std::cout << "Derivative gain needs to be larget than zero." << std::endl;
return 0;
}
drone<servo, pid> d(std::vector<servo>({
servo(4, 1.2, maxPwm, pins::instance().pwm),
servo(5, 1.2, maxPwm, pins::instance().pwm),
servo(6, 1.2, maxPwm, pins::instance().pwm),
servo(7, 1.2, maxPwm, pins::instance().pwm) }),
std::vector<pid>({
pid(pGain, iGain, dGain),
pid(pGain, iGain, dGain),
pid(pGain, iGain, dGain) }));
stupid = &d;
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = ting;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
d.run();
}
//main///////////////////////////////////////////////////////////////////////////////////////////////
int main(void) {
//initialize the IO port pins
//I like to use binary because it's easy to see right away which way pins are set.
//Setting a bit to zero makes its corresponding pin behave as an input,
//setting to one makes it an output.
DDRB = 0b001111;
//Make all the output pins go low:
PORTB=0;
//Now we're ready for some real stuff!
//Infinite loop. The code does whatever is written between here
//and the corresponding curly brace
//until power is removed or the chip is reset.
while(1){
//define a variable and fill it with a random number from 1 to 30.
//this will determine how many times the loop is run before the rest period.
unsigned char loop=limited_random(1,30);
//start the main wiggling loop, using the number from just above:
for(;loop>0;loop--){
//how many times to call the servo routine
char reps=limited_random(3,25);
//the angle for the servos.
char x=limited_random(0,180);
char y=limited_random(0,180);
char z=limited_random(0,180);
//start the loop that sends a volley of pulses to the servos:
for(;reps>0;reps--){
//Actually send the servo pulses now:
servo(SERVO1,x);
servo(SERVO2,y);
servo(SERVO3,z);
//Delay a little bit so the servo has
//time to execute the command before we send another one:
delay_us(10000);
}
}
led_on();
delay_us(limited_random(1,30)*1000);
led_off();
}
}
void ServoInitFunc(void) {
servo(0);
ConfigIntTimer1(T1_INT_PRIOR_1 & T1_INT_ON);
//1msec(4/80Mhz×1×200=0.01msec)
OpenTimer1(T1_ON & T1_GATE_OFF & T1_PS_1_1 & T1_SYNC_EXT_OFF & T1_SOURCE_INT,200-1);
}
void main(void)
{
DDRE=0xFF;
//DDRE |= (1<<PE3) | (1<<PE4) | (1<<PE5);
//int x=63;
intialize();
while(1)
{
servo(1,60);
_delay_ms(500);
servo(1,150);
_delay_ms(500);
}
}
void tests::servo_drive_interaction() {
VS11 servo(3);
servo.setGoal(0);
delay(1000);
servo.setGoal(M_PI/2);
delay(1000);
Serial.println("0");
{
Drive d;
d.setMLSpeed(100);
d.setMRSpeed(100);
delay(1000);
servo.setGoal(0);
delay(1000);
d.setMLSpeed(-100);
d.setMRSpeed(-100);
delay(1000);
servo.setGoal(M_PI/2);
delay(1000);
d.setMLSpeed(0);
d.setMRSpeed(0);
}
}
/* ==============================================
main task routine
============================================== */
int main(void) {
pool_memadd((uint32_t)pool, sizeof(pool));
#ifdef DEBUG
dbg.start();
#endif
int val;
float degrees;
CAdc vr(AD5); // use A5 for VR
CServo servo(PWM1); // use PWM1 for Servo Motor
vr.begin();
servo.begin();
// Enter an endless loop
while(1){
val = vr.read(0.1, 8);
degrees = AD2Degrees(val);
servo = degrees;
#ifdef DEBUG
if ( dbg.isDebugMode() ) {
dbg.println("ADC=%d, Degrees=%f\n", val, degrees);
}
#endif
sleep(100);
}
return 0 ;
}
void Output::parse(char *c){
int n,s,m,a;
Output *t;
switch(sscanf(c,"%d %d %d %d",&n,&s,&m,&a)){
case 2: // argument is string with id number of output followed by zero (LOW) or one (HIGH)
t=get(n);
if(t!=NULL)
t->activate(s);
else
INTERFACE.print("<X>");
break;
case 3: // argument is string with id number of output followed by a pin number and invert flag
create(n,s,m,1);
break;
case 4:
servo(n, m, a);
break;
case 1: // argument is a string with id number only
remove(n);
break;
case -1: // no arguments
show(1); // verbose show
break;
}
}
void expand() {
servo(1, STL);
delayMs(100);
servo(5, STR);
delayMs(100);
/* servo(4, SBL);
delayMs(100);
servo(6, SBR);
delayMs(100);
*/
servoOff(1);
// servoOff(4);
servoOff(5);
// servoOff(6);
}
int main() {
Dynamixel servo(3, 9600);
usleep(100000);
servo.set_position(1023);
usleep(1000000);
servo.set_position(0);
return 0;
}
void GatePos(int deg)
{
int i;
for (i = 0; i < 2; i++)
{
servo(i, WProportion(deg, 0, 90, rgcAngle[2*i], rgcAngle[2*i+1]));
}
}
void Gate(int fDown)
{
int i;
for (i = 0; i < 2; i++)
{
servo(i, rgcGate[i*2 + fDown]);
}
fGateDown = fDown;
}
// <== Eigene Funktion und Bedingungen formulieren / schreiben
void fahren1(void){
ar = abstandrechts;
av = abstandvorne;
al = abstandlinks;
if (av > 50)
{
back = 0;
}
if ((av < 45) || (back == 1))
{
back = 1;
fahr(-20);
if (ar > al)
{
servo(-10);
}
else
{
servo(10);
}
}
else
{
fahr(30);
if (av < 200)
{
fahr(18);
if (ar > al)
{
servo(10);
}
else
{
servo(-10);
}
}
else if (al > 60)
{
servo(((ar)*m1)/m2 - 19);
}
else if (ar > 60)
{
servo(((ar)*m1)/m2 - 19);
}
else
{
servo(((ar - al)*10)/30);
}
}
}
void Enriduino::start(){
if (Serial.available()){
serial[i++] = Serial.read();
}
if (serial[i-1]==126){
i=0;
pinmode();
Write();
Read();
AWrite();
ARead();
servo();
servoWrite();
}
}
void tests::servo() {
VS11 servo(3);
servo.setGoal(0);
delay(500);
Serial.println("+90");
servo.setGoal(M_PI/2);
delay(500);
Serial.println("-90");
servo.setGoal(-M_PI/2);
delay(500);
Serial.println("0");
servo.setGoal(0);
}
void ServoRange()
{
int pos;
int portServ = 0;
while (!stop_button())
{
if (start_button())
{
portServ = 1 - portServ;
while (start_button());
}
pos = ScaleKnob(30, 3960);
printf("Servo: %d\n", pos);
servo(portServ, pos);
}
while (stop_button());
}
void main()
{
unsigned char pos=20;
while(1)
{
lcd("Position: %d ",pos);
servo(0,pos);
if(sw1()==0)
{
pos++;
sleep(100);
}
if(sw2()==0)
{
pos--;
sleep(100);
}
}
}
void Output::activate(int s){
data.oStatus=(s>0); // if s>0, set status to active, else inactive
if (data.pin >= 62 && data.pin < 70)
servo(data.pin - 62, data.oStatus ^ bitRead(data.iFlag,0));
else
digitalWrite(data.pin,data.oStatus ^ bitRead(data.iFlag,0)); // set state of output pin to HIGH or LOW depending on whether bit zero of iFlag is set to 0 (ACTIVE=HIGH) or 1 (ACTIVE=LOW)
if(num>0)
EEPROM.put(num,data.oStatus);
INTERFACE.print("<Y");
INTERFACE.print(data.id);
if(data.oStatus==0)
INTERFACE.print(" 0>");
else
INTERFACE.print(" 1>");
Serial2.print("<Y");
Serial2.print(data.id);
if(data.oStatus==0)
Serial2.print(" 0>");
else
Serial2.print(" 1>");
}
int main(void){
Led0 led0;
Sw0 sw0;sw0.setupDigitalIn();
Sw1 sw1;sw1.setupDigitalIn();
Sw2 sw2;sw2.setupDigitalIn();
Sw3 sw3;sw3.setupDigitalIn();
Blink blink0(led0);blink0.setup();blink0.time(200);
Serial1 serial1; serial1.setup(115200);
Serial0 serial0;
Servo servo(serial0);servo.setup();
int data=0;
int flag=0;
float deg=0.00;
long long int time=0;
long long int timer=0;
long long int servotime=0;
int i=0;
while(1){
blink0.cycle();
for(i=0;i<1000;i++){
servo.setAngle(0,130);
while(serial0.charAvailable()){
servo.rec(0,serial0.readChar());
//serial1.printf("%d\n",serial0.readChar());
}
wait(1);
}
for(i=0;i<1000;i++){
servo.setAngle(0,-130);
while(serial0.charAvailable()){
servo.rec(0,serial0.readChar());
}
wait(1);
}
}
}
int main(void)
{
Led3 led0;
Sw0 sw0;sw0.setupDigitalIn();
Blink blink0(led0);blink0.setup();blink0.time(200);
Pwm0 pwm0;
Servo servo(pwm0);servo.setup(20,270,1.5,2.3);
Serial0 serial;
serial.setup(115200);
float deg=0;
float duty=0;
int flag=0;
long long int time=0;
long long int tim=0;
//Set_duty(SERVO_TIM_CH,0);
wait(500);
while(1){
blink0.cycle();
for(deg=-25;deg<115;deg+=10){
servo.duty(deg);
servo.cycle();
//led0.digitalToggle();
//serial0.printf("%f\n",deg);
wait(25);
}
for(deg=115;deg>-25;deg-=10){
servo.duty(deg);
servo.cycle();
//led0.digitalToggle();
// serial0.printf("%f\n",deg);
wait(25);
}
/*if(millis()-tim>=10){
tim=millis();
if(serial.charAvailable()){
flag=true;
char key=serial.readChar();
serial.printf("key %c\n",key);
if(key=='d'){
deg+=5.0;
deg=floatlimit(-25,deg,115);
}else if(key=='a'){
deg-=5.0;
deg=floatlimit(-25,deg,115);
}else if(key=='w'){
deg=0;
}
serial.printf("deg %.2f",deg);
servo.duty(deg);
}
}
if(flag)servo.cycle();*/
/*if(millis()-time>=500){
time=millis();
if(!sw0.digitalRead()){
switch (flag) {
case 0:
flag=1;
deg=-135;
duty=0.7/30;
break;
case 1:
flag=2;
deg=135;
duty=2.3/30;
break;
case 2:
flag=0;
deg=0;
duty=1.5/30;
break;
default:
break;
}
servo_move(deg);
//Set_duty(SERVO_TIM_CH,duty);
serial.printf("%f\n",deg);
}
}*/
}
}
void test_oscope(void)
{
int se0,se1,se2,se3,se4,se5,se6,se7,se8;
int se9,se10,se11,se12,se13,se14,se15;
LED_on();
if (button_pressed())
LED_off();
//pulse each fast for oscope testing
//right row
servo(PORTC,0,360);
servo(PORTC,1,360);
servo(PORTC,2,360);
servo(PORTC,3,360);
servo(PORTC,4,360);
servo(PORTC,5,360);
servo(PORTC,6,360);
servo(PORTC,7,360);
servo(PORTJ,6,360);
servo(PORTA,0,360);
servo(PORTA,1,360);
servo(PORTA,2,360);
servo(PORTA,3,360);
servo(PORTA,4,360);
servo(PORTA,5,360);
servo(PORTA,6,360);
servo(PORTA,7,360);
//left row
servo(PORTE,2,360);
servo(PORTE,3,360);
servo(PORTE,4,360);
servo(PORTE,5,360);
servo(PORTE,6,360);
servo(PORTE,7,360);
servo(PORTH,2,360);
servo(PORTH,3,360);
servo(PORTH,4,360);
servo(PORTH,5,360);
servo(PORTH,6,360);
//top row
se0=a2dConvert8bit(0);
se1=a2dConvert8bit(1);
se2=a2dConvert8bit(2);
se3=a2dConvert8bit(3);
se4=a2dConvert8bit(4);
se5=a2dConvert8bit(5);
se6=a2dConvert8bit(6);
se7=a2dConvert8bit(7);
se8=a2dConvert8bit(8);
se9=a2dConvert8bit(9);
se10=a2dConvert8bit(10);
se11=a2dConvert8bit(11);
se12=a2dConvert8bit(12);
se13=a2dConvert8bit(13);
se14=a2dConvert8bit(14);
se15=a2dConvert8bit(0x0f);//15
rprintfInit(uart1SendByte);
rprintf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d \r\n",se0,se1,se2,se3,se4,se5,se6,se7);
rprintf("8:%d 9:%d 10:%d 11:%d 12:%d 13:%d 14:%d 15:%d \r\n",se8,se9,se10,se11,se12,se13,se14,se15);
rprintfInit(uart0SendByte);
rprintf(" 0");
rprintfInit(uart2SendByte);
rprintf(" 2");
rprintfInit(uart3SendByte);
rprintf(" 3");
delay_cycles(65500);
}
// This closes the claw;
void Robot::closeClaws()
{
servo(SERVO_RIGHT, CLOSERIGHTANGLE);
servo(SERVO_LEFT, CLOSELEFTANGLE);
}
void Robot::openClaws()
{
servo(SERVO_RIGHT, OPENRIGHTANGLE);
servo(SERVO_LEFT, OPENLEFTANGLE);
}
void forward(unsigned int delay)
{
servo(0,SERVO_MID+SERVO_SPEED);
servo(1,SERVO_MID-SERVO_SPEED);
sleep(delay);
}
void turn_right(unsigned int delay)
{
servo(0,SERVO_MID+SERVO_SPEED);
servo(1,SERVO_MID+SERVO_SPEED);
sleep(delay);
}
void turn_left(unsigned int delay)
{
servo(0,SERVO_MID-SERVO_SPEED);
servo(1,SERVO_MID-SERVO_SPEED);
sleep(delay);
}
