F3RC1班
自動機
Diff: main.cpp
- Revision:
- 1:651bd0514eb9
- Parent:
- 0:1f542f8756d6
- Child:
- 2:c8c264a9a7f6
--- a/main.cpp Tue Aug 22 02:39:13 2017 +0000
+++ b/main.cpp Wed Sep 06 02:46:11 2017 +0000
@@ -6,253 +6,344 @@
SPISlave spi(PB_15,PB_14,PB_13,PB_12);
Ec Ec1(PB_6,PC_7,NC,1048,0.05);
- Ticker ticker;
- Ticker ticker2;
- DigitalIn button(USER_BUTTON);
- Serial pc(USBTX,USBRX);
-
-SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //left
-SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //right
+Ticker ticker;
+Ticker ticker2;
+DigitalIn button(USER_BUTTON);
+Serial pc(USBTX,USBRX);
+
+SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //左
+SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //右
+
+Timer timer;
-
-
+int kai=0;
int n=1;
+int ang=0;
+int a=0;
+int c=0;
+int d=0;
+int dis=0;
+double V=0;
+double v();
+int e=0;
+int f=0;
+
-void calOmega() //角速度計算関数
+void calOmega() //角速度計算関数
+{
+ motor1.CalOmega();
+ motor2.CalOmega();
+ Ec1.CalOmega();
+}
+
+void Ang() //Nucleoリセット時からの機体角度(0~3600)
{
- motor1.CalOmega();
- motor2.CalOmega();
- Ec1.CalOmega();
+ if(spi.receive())ang=spi.read() - d;
+
+ if(ang < 0)ang += 3600;
+ if(ang > 3600)ang-=3600;
+
}
+
+void print()
+{
+ if(kai>=500) {
+ //pc.printf("count1=%d\r\n",dis);
+ // pc.printf("count=%f ",motor1.getOmega());
+ // pc.printf("count2=%f\r\n ",motor2.getOmega());
+ //pc.printf("duty1=%f ",motor1.duty);
+ //pc.printf("duty2=%f\r\n",motor2.duty);
+ //pc.printf("d=%d",d);
+ //pc.printf("v=%f\r\n",v());
+ pc.printf("ang=%d\r\n",ang);
+ kai=0;
+ }
+ kai++;
+}
+
+
void st()
{
- int kai=0;
- while(kai<50000){
motor1.stop();
motor2.stop();
- kai=kai + 1;
- }
-}
-
-/*void print()
-{
- // pc.printf("n=%d",n);
- // pc.printf("count1=%d",Ec1.getCount());
- // pc.printf("gyro=%d",spi.read());
- pc.printf("count1=%f ",motor1.getOmega());
- pc.printf("count2=%f ",motor2.getOmega());
- pc.printf("duty1=%f ",motor1.duty);
- pc.printf("duty2=%f\r\n",motor2.duty);
-}*/
-
-void str(int a) //
-{
-int kai=0;
-
- while(n==1){
- int dis=0.301*Ec1.getCount();
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- pc.printf("count1=%f ",motor1.getOmega());
- pc.printf("count2=%f ",motor2.getOmega());
- pc.printf("duty1=%f ",motor1.duty);
- pc.printf("duty2=%f\r\n",motor2.duty);
- kai=0;
- // }
- }
- if(dis < a){ //距離
- motor1.Sc(5);
- motor2.Sc(5);
- }else{
- n=2;
- }
- kai++;
- //pc.printf("kai=%d\r\n",kai);
-
-}
-st();
-}
-void back(int a)
-{
- int kai=0;
- while(n==2){
- int dis=0.301*Ec1.getCount();
- kai++;
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- pc.printf("count1=%f ",motor1.getOmega());
- pc.printf("count2=%f ",motor2.getOmega());
- pc.printf("duty1=%f ",motor1.duty);
- pc.printf("duty2=%f\r\n",motor2.duty);
- kai=0;
- // }
- }
- if(dis > a){
- motor1.Sc(-5);
- motor2.Sc(-5);
- }else{
- motor1.Sc(0);
- motor2.Sc(0);
- n=1;
- }
- }
- st();
+ wait(0.5);
}
-void turnR(int b,int c)
+
+
+void str(int a)
{
- int kai=0;
- int ang=0;
- while(n==2){
+
+ while(n==1) {
+ dis=0.301*Ec1.getCount();
+ if(dis < a) {
+ V=15;
+ e=400;
- kai++;
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- // pc.printf("count1=%f ",motor1.getOmega());
- pc.printf("count2=%f ",motor2.getOmega());
- // pc.printf("duty1=%f ",motor1.duty);
- pc.printf("duty2=%f\r\n",motor2.duty);
- // pc.printf("ang=%f\r\n",ang);
- kai=0;
- // }
- }
- /* if(spi.receive()){
- pc.printf("count1=%d",Ec1.getCount());
- pc.printf("gyro=%d\r\n",spi.read());
- }*/
- if(spi.receive()){
- ang=spi.read();
-// pc.printf("ang=%d\r\n",ang);
- }
- if((ang < b) || (ang > c)){ //角
- motor1.Sc(5);
- motor2.Sc(-5);
-
- }else{
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- n=1;
- }
-}
-st();
+ if(dis > a-e) {
+ V = (a - dis)*V/e;
+ if(V<2)V=2;
+ }
+
+ motor1.Sc(V);
+ motor2.Sc(V);
+
+ } else {
+ n=2;
+ }
+ }
+ st();
}
-void turnL(int b,int c)
+
+void back(int a)
{
- int kai=0;
- int ang=0;
- while(n==2){
+ e=400;
+
+ motor1.setDOconstant(20.7);
+ motor2.setDOconstant(20.3);
+
- kai++;
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- pc.printf("count1=%f ",motor1.getOmega());
- // pc.printf("count2=%f ",motor2.getOmega());
- pc.printf("duty1=%f\r\n ",motor1.duty);
- // pc.printf("duty2=%f\r\n",motor2.duty);
- // pc.printf("ang=%f\r\n",ang);
- kai=0;
- // }
- }
-/* if(spi.receive()){
- pc.printf("n=%d",n);
- pc.printf("count1=%d",Ec1.getCount());
- pc.printf("gyro=%d\r\n",spi.read());
- }*/
- if(spi.receive()){
- ang=spi.read();
-// pc.printf("ang=%d\r\n",ang);
- }
- if((ang < b) || (ang > c)){ //角度
- motor1.Sc(-5);
- motor2.Sc(5);
- }else{
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- n=1;
- }
-
- }
- st();
+ while(n==2) {
+ V=15;
+ dis=0.301*Ec1.getCount();
+ if(dis > a) {
+ if(dis < a+e ) {
+ V = (a - dis)*V/e;
+ if(V<2)V=2;
+ }
+ motor1.Sc(-V);
+ motor2.Sc(-V);
+ } else {
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+ }
+ st();
+ motor1.setDOconstant(19.7);
+ motor2.setDOconstant(21.3);
}
-int main() {
+void turn(int b)
+{
+ c=0;
+ f=700;
+
+ while(n==2) {
+ V=5;
+ print();
+ if(b >= 0) {
+
+
+ a = ang - b; //角度(目標値から±1800)
+
+ if(a < -1800) {
+ a += 3600;
+ } else if(a > 1800) {
+ a -= 3600;
+ }
+ print();
+ if(c==1 && a > 20) {
+ motor1.Sc(-v());
+ motor2.stop();
+ } else if(c==2 && a < -20) {
+ motor1.stop();
+ motor2.Sc(-v());
+ } else if(a < -f) {
+ motor1.Sc(V);
+ motor2.stop();
+ } else if(a > f) {
+ motor1.stop();
+ motor2.Sc(V);
+ } else if(a < -10) {
+ motor1.Sc(v());
+ motor2.stop();
+ c=1;
+ } else if(a > 10) {
+ motor1.stop();
+ motor2.Sc(v());
+ c=2;
+ } else {
+ Ec1.reset();
+ motor1.stop();
+ motor2.stop();
+ n=1;
+ }
+
+ } else {
+ a = ang + b;
+
+ if(a < -1800) {
+ a += 3600;
+ } else if(a > 1800) {
+ a -= 3600;
+ }
+
+ if(c==1 && a > 20) {
+ motor1.stop();
+ motor2.Sc(v());
+ } else if(c==2 && a < -20) {
+ motor1.Sc(v());
+ motor2.stop();
+ } else if(a < -f) {
+ motor1.stop();
+ motor2.Sc(-V);
+ } else if(a > f) {
+ motor1.Sc(-V);
+ motor2.stop();
+ } else if(a < -10) {
+ motor1.stop();
+ motor2.Sc(-v());
+ c=1;
+ } else if(a > 10) {
+ motor1.Sc(-v());
+ motor2.stop();
+ c=2;
+ } else {
+ Ec1.reset();
+ motor1.stop();
+ motor2.stop();
+ n=1;
+ }
+ }
+ }
+ while(1){
+ print();
+ }
+}
+
+double v(){
+ if(a < 0)a=-a;
+ V=(a-10)*V/f;
+ if(V < 0.3)V=0.3;
+ return V;
+}
+
+
+int main()
+{
servo.period_ms(20);
spi.format(16,3);
spi.frequency(1000000);
-
+
ticker.attach(&calOmega,0.05);
- // ticker2.attach(&print,0.5);
+ ticker2.attach(&Ang,0.05);
+
+ motor1.setPDparam(0.3,0.4); //PDパラメータを設定
+ motor2.setPDparam(0.3,0.2); //PDパラメータを設定
+
+ motor1.setDOconstant(19.6);
+ motor2.setDOconstant(22.3);
+
+
+ sw.mode(PullUp);
+
+
+
+
- motor1.setPDparam(0.55,0.5); //PDパラメータを設定
- motor2.setPDparam(0.6,0.5); //PDパラメータを設定
-
- motor1.setDOconstant(13.1);
- motor2.setDOconstant(15.8);
-
-
- sw.mode(PullUp);
-
+ if(sw==1) { //スタートゾーン１
+ servo.pulsewidth_us(900);
+ wait(0.5);
+ d=ang; //初期角度
+ n=2;
+ turn(2700);
+
+ /* turn(3100);
+ str(950);
+ turn(2410);
+ str(1070);
+ turn(2700);
+ servo.pulsewidth_us(1800);
+ str(870);
+ servo.pulsewidth_us(900);
+ wait(1);
+ back(0);
+ n=2;
+ turn(-2430);
+ n=2;
+ back(-1080);
+ n=2;
+ turn(-2700);
+ n=2;
+ back(-970);
+ n=2;
+ turn(1800);
+ str(500);
+ servo.pulsewidth_us(1800);
+ wait(0.3);
-
- if(sw==1){ //スタートゾーン１
-
- servo.pulsewidth_us(900);
- str(540);
- turnL(2650,2750);
- str(1481);
- turnL(1750,1850);
- str(540);
- turnR(2650,2750);
- str(1481);
- servo.pulsewidth_us(1800);
- wait(0.5);
- back(0);
- n=2;
- turnR(3550,3600);
- str(540);
- turnR(850,950);
- str(1481);
- turnR(1750,1850);
- str(570);
- servo.pulsewidth_us(900);
- back(450);
- servo.pulsewidth_us(1800);
- str(570);
-
- //押し出し
-
- }else{ //スタートゾーン２
- servo.pulsewidth_us(900);
- str(540);
- servo.pulsewidth_us(1800);
- wait(0.5);
- turnL(850,950);
- str(1481);
- turnL(0,50);
- str(540);
- turnR(850,950);
- str(1481);
- turnR(1750,1850);
- str(570);
- servo.pulsewidth_us(900);
- back(450);
- servo.pulsewidth_us(1800);
- str(570);
-
-
-
+ motor1.setDOconstant(20.7);
+ motor2.setDOconstant(20.3);
+ while(n==2) {
+ dis=0.301*Ec1.getCount();
+ if(dis > 370) {
+ motor1.Sc(-15);
+ motor2.Sc(-15);
+ } else {
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+ }
+ st();
+ motor1.setDOconstant(19.7);
+ motor2.setDOconstant(21.3);
+
+ servo.pulsewidth_us(900);
+ str(500);
+
+ while(n==2) {
+ dis=0.301*Ec1.getCount();
+ if(dis > 370) {
+ motor1.Sc(-15);
+ motor2.Sc(-15);
+ } else {
+ motor1.stop();
+ motor2.stop();
+ n=1;
+ }
+ }*/
+
+ } else { //スタートゾーン２
+ servo.pulsewidth_us(1800);
+ wait(0.5);
+ d=ang - 1800;
+ str(450);
+ servo.pulsewidth_us(900);
+ wait(1);
+ turn(880);
+ str(820);
+ turn(610);
+ str(1080);
+ turn(1320);
+ str(930);
+ turn(1800);
+ servo.pulsewidth_us(1800);
+ n=2;
+ wait(0.3);
-
- }
-
-
-} //・距離とエンコーダ出力の関係 ・ジャイロの値
+ motor1.setDOconstant(20.7);
+ motor2.setDOconstant(20.3);
+ while(n==2) {
+ dis=0.301*Ec1.getCount();
+ if(dis > -130) {
+ motor1.Sc(-15);
+ motor2.Sc(-15);
+ } else {
+ motor1.stop();
+ motor2.stop();
+ n=1;
+ }
+ }
+ }
+
+}
+