Revision 5:1c95260515c1, committed 2016-09-03

Comitter:

choutin

Date:

Sat Sep 03 06:51:48 2016 +0000

Parent:

4:dcb03da10fa7

Commit message:

test for you

Changed in this revision

--- a/main.cpp	Thu Sep 01 06:23:53 2016 +0000
+++ b/main.cpp	Sat Sep 03 06:51:48 2016 +0000
@@ -1,62 +1,132 @@
-//受渡前での妨害に対応したい
-#include"header.h"
-Serial pc(SERIAL_TX, SERIAL_RX);
+#include "mbed.h"
+
+PwmOut pwmarm(PC_6);
+PwmOut pwmhand(PC_8);
+DigitalOut led(LED1);
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2ccw(PB_14);
+PwmOut M2cw(PB_13);
+float PERIOD=20000;
+
+void initmotor()
+{
+
+
+    M1cw.period_us(256);
+    M1ccw.period_us(256);
+    M2cw.period_us(256);
+    M2ccw.period_us(256);
+
+}
+
+void move(int left,int right)
+{
+
+    float rightduty,leftduty;
 
-void movelength(int length){
-    int px,py,pt;
-    update(-right.getPulses(), left.getPulses());
-    px=coordinateX();
-    py=coordinateY();
-    pt=coordinateTheta();
-    
-    move(30,30);
-    while(1){ 
-        update(-right.getPulses(), left.getPulses());
-        if((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY())>length*length){break;}
-        
-        }
+    if(right>256) {
+        right=256;
+    }
+    if(left>256) {
+        left=256;
+    }
+    if(right<-256) {
+        right=-256;
+    }
+    if(left<-256) {
+        left=-256;
+    }
+
+    rightduty=right/256.0;
+    leftduty=left/256.0;
+    if(right>0) {
+        M1cw.write(1-rightduty);
+        M1ccw.write(1);
+    } else {
+        M1cw.write(1);
+        M1ccw.write(1+rightduty);
+    }
+
+    if(left>0) {
+        M2cw.write(1-leftduty);
+        M2ccw.write(1);
+    } else {
+        M2cw.write(1);
+        M2ccw.write(1+leftduty);
+    }
 }
 
 
-void turncw(int degree) {
-  initencorder();
-  initmotor();
-  
-  int rad=0,np=1;
-  if(degree<0){np=-1;}
-  rad=degree*PI/180;
-  move(30*np,-30*np);
-  float t,theta;
-  int x,y;
-  update(-right.getPulses(), left.getPulses());
-  t=coordinateTheta();
-  while(1){
+void close_hand(void){
+    int i,degree;
+
+   pwmhand.period_ms(20);        //20ms
+   
+       degree=175;
 
-      pc.printf("x:%d y:%d t:%f \n\r",x,y,theta*180/PI);
-      x=coordinateX();
-      y=coordinateY();
-      theta=coordinateTheta();
-      update(-right.getPulses(), left.getPulses());
-      if(theta-t > np*rad){break;}
-      if(theta-t < (-1)*np*rad){break;}
-       //   moveto (30,30);
-       /*
-               move(30,30);
-          update(-right.getPulses(), left.getPulses());
-          pc.printf("x:%d y:%d t:%f \n\r",coordinateX(), coordinateY(), coordinateTheta());
-          if(coordinateX()>300){
-              while(1){*/
-             
-  }
-  move(0,0);
+       i=500+degree*1900/180;
+       pwmhand.write(i/PERIOD); 
+       
+       
+}
+
+void close_arm(void) {
+    int i,degree;
+
+   pwmarm.period_ms(20);        //20ms
+   
+       degree=160;
+
+       i=500+degree*1900/180;
+       pwmarm.write(i/PERIOD); 
+       
+       
 }
 
 
+
+void open_hand(void) {
+    int i,degree;
+
+   pwmhand.period_ms(20);        //20ms
+   
+       degree=90;
+
+       i=500+degree*1900/180;
+       pwmhand.write(i/PERIOD); 
+       
+       
+}
+
+
+
+void open_arm(void) {
+    int i,degree;
+
+   pwmarm.period_ms(20);        //20ms
+   
+       degree=10;
+
+       i=500+degree*1900/180;
+       pwmarm.write(i/PERIOD); 
+       
+       
+}
+
 int main(){
-    
-    movelength(600);
-    turncw(90);
-    movelength(1200);
-    turncw(90);
-    
+ move(-30,-30);
+ while(1){
+ open_arm();
+ close_hand();
+ move(1,1);
+ led=1;
+ wait(1);
+ close_arm();
+ open_hand();
+ move(-255,-255);
+ led=0;
+ wait(1);
+ }
+ return 0;   
 }
\ No newline at end of file