Diff: prime.h

Revision:

1:a1e592eca305

Parent:

0:df2659fd8031

Child:

2:1a2984dfac3e

--- a/prime.h	Sun Aug 28 17:03:34 2016 +0000
+++ b/prime.h	Tue Aug 30 06:51:41 2016 +0000
@@ -1,3 +1,4 @@
+#include "QEI.h"
 /*
 primeではPINを制御する関数を扱う。
 
@@ -21,14 +22,39 @@
 AnalogIn armadj(A2);
 AnalogIn handadj(A3);
 
-DigitalIn team(PC_3);
+DigitalIn teamSW(PC_11);
+DigitalOut teamledblue(PC_10);
+DigitalOut teamledred(PC_12);
+
 DigitalOut errorled(LED1);
 
 
-DigitalIn phase1(PC_14);
-DigitalIn phase2(PC_15);
-DigitalIn phase4(PH_0);
-DigitalIn phase8(PH_1);
+DigitalIn phase1(PB_8);
+DigitalIn phase2(PC_9);
+DigitalIn phase4(PB_9);
+DigitalIn phase8(PD_2);
+
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2cw(PB_14);
+PwmOut M2ccw(PB_13);
+
+DigitalOut PINW(PA_3);
+DigitalOut PINX(PA_2);
+DigitalOut PINY(PA_10);
+DigitalOut PINZ(PB_3);
+
+QEI right (PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
+QEI left  (PH_0, PC_14, NC, 100, QEI::X2_ENCODING);
+
+DigitalOut encordervcc1(PA_10);
+DigitalOut encordervcc2(PC_15);
+
+void initencorder(void){
+    encordervcc1=1;
+    encordervcc2=1;
+    }
+
 
 
 void close_hand(void) {
@@ -93,11 +119,7 @@
 void step(int degree){
     
     
-    DigitalOut PINW(PA_3);
-    DigitalOut PINX(PA_2);
-    DigitalOut PINY(PA_10);
-    DigitalOut PINZ(PB_3);
-    
+
     int puls_times=0;
 
     if(degree>0){
@@ -210,7 +232,7 @@
              //                 0        1         2       3
 }
 
-
+/*
 void move(int right,int left){
     
     if(right>256){right=256;}
@@ -273,12 +295,42 @@
         M2ccw.pulsewidth_us(256);
     }
 }
+*/
 
-int blue(){
-    int x;
-    x=(-1)*team;
-    return x;
+void initmotor(){
+
+    
+    M1cw.period_us(256);
+    M1ccw.period_us(256);
+    M2cw.period_us(256);
+    M2ccw.period_us(256);
+    
+}
+
+void move(int right,int left){
+    
+    if(right>256){right=256;}
+    if(left>256){left=256;}
+    if(right<-256){right=-256;}
+    if(left<-256){left=-256;}
+
+    
+    //回さないモーターにはperiod=widthを出力
+    if(right>0){
+        M1cw.pulsewidth_us(256-right);
+        M1ccw.pulsewidth_us(256);
+    }else{
+        M1cw.pulsewidth_us(256);
+        M1ccw.pulsewidth_us(256+right);
     }
+    if(left>0){
+        M2cw.pulsewidth_us(256-left);
+        M2ccw.pulsewidth_us(256);
+    }else{
+        M2cw.pulsewidth_us(256);
+        M2ccw.pulsewidth_us(256+left);
+    }
+}
     
     
 int phase(void){