int main(void)
{
long long int time=0;
Led0 led;Blink blink0(led);blink0.setup();blink0.time(100);
A0 a0;Blink blink1(a0);blink1.setup();blink1.time(100);
Serial0 serial;serial.setup(115200);
Serial1 serial1;
Enc0 l;Enc1 r;Enc2 b;
RobotCenter center(l,r,b);center.setup();
Can0 can;
DualShock ps3(serial1);
ps3.setStickTolerance(0.1);
//DualShock ps3(serial1);ps3.setup();
/*Cw4 cw4;Ccw4 ccw4;Pwm4 pwm4;Md md4(cw4,ccw4,pwm4);
Cw5 cw5;Ccw5 ccw5;Pwm5 pwm5;Md md5(cw5,ccw5,pwm5);
Cw6 cw6;Ccw6 ccw6;Pwm6 pwm6;Md md6(cw6,ccw6,pwm6);
Cw7 cw7;Ccw7 ccw7;Pwm7 pwm7;Md md7(cw7,ccw7,pwm7);*/
CW0 cw4;CCW0 ccw4;Pwm0 pwm4;Md md4(cw4,ccw4,pwm4);
CW1 cw5;CCW1 ccw5;Pwm1 pwm5;Md md5(cw5,ccw5,pwm5);
CW2 cw6;CCW2 ccw6;Pwm2 pwm6;Md md6(cw6,ccw6,pwm6);
CW3 cw7;CCW3 ccw7;Pwm3 pwm7;Md md7(cw7,ccw7,pwm7);
Move move(md4,md5,md6,md7,center,ps3);move.setup();
Debug debug(serial,center,ps3);debug.setup();
while(1){
blink0.cycle();
center.cycle();
ps3.cycle();
//move.manualModeCycle();
debug.cycle();
if(millis()-time>=100){
time=millis();
/*serial.printf("lx:%.4f",ps3.lx());
serial.printf("ly:%.4f",ps3.ly());
serial.printf("rx:%.4f",ps3.rx());
serial.printf("ry:%.4f",ps3.ry());
serial.printf("u:%.4f",ps3.up());
serial.printf("r:%.4f",ps3.right());
serial.printf("d:%.4f",ps3.down());
serial.printf("l:%.4f",ps3.left());
serial.printf("tri:%.4f",ps3.triangle());
serial.printf("cir:%.4f",ps3.circle());
serial.printf("cro:%.4f",ps3.cross());
serial.printf("squ:%.4f",ps3.square());
serial.printf("l1:%.4f",ps3.l1());
serial.printf("l2:%.4f",ps3.l2());
serial.printf("r1:%.4f",ps3.r1());
serial.printf("r2:%.4f",ps3.r2());
serial.printf("start:%.4f",ps3.start());
serial.printf("select:%.4f",ps3.select());
serial.printf("dis:%d\n",ps3.disconnect());*/
//serial.printf("ang,% .4f, x,% .4f, y,% .4f\n",rtod(center.getAngle()),center.x(),center.y());
}
}
return 0;
}
void loop() {
// Print help
// _T() makes string to be placed on program memory (saves ram)
// This should be used for all static strings
serial << _T("Serial terminal. Commands:\n"
" b - begin action\n"
" e - end action\n"
"\n"
"command: ");
// Endl would call flush, but it isn't used in above code, so it's called here instead
serial.flush();
// read command
char c = -1;
serial >> c; // read byte
serial << endl; // write newline
switch (c) {
case ASCII::NUL:
case ASCII::ENQ:
// ENQ == ENQUIRY == "Who are you?"
break;
case 'b':
led = true; // turn led on (start action)
serial << _T("Action has begun") << endl;
break;
case 'e':
led = false; // turn led off (end action)
serial << _T("Action has ended") << endl;
break;
default:
serial << _T("Invalid command: ") << c << endl;
}
}
void setup() {
led.mode = OUTPUT;
serial.setup(9600, DATA_EIGHT, STOP_ONE, PARITY_DISABLED);
}
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);
}
}*/
}
}