Diff: main.cpp

Revision:

1:681efc14eba9

Parent:

0:158455b60200

--- a/main.cpp	Sat Jan 19 12:13:21 2019 +0000
+++ b/main.cpp	Sat Jan 19 14:55:42 2019 +0000
@@ -1,46 +1,46 @@
 #include "mbed.h"
 #include "speedservo.h"
-
+ 
 //肘
 Servo RFelbow(PA_13);
 Servo LFelbow(PA_15);
 Servo RBelbow(PC_2);
 Servo LBelbow(PC_13);
-
+ 
 //膝
 Servo RFknee(PA_14);
 Servo LFknee(PC_1);
 Servo RBknee(PC_3);
 Servo LBknee(PC_7);
-
+ 
 //肩
 Serial SerialServo(PC_4,PA_10);
-
+ 
 DigitalIn Dmode(PB_13);//歩行方向変更
 DigitalIn start_button(PB_2);
-
+ 
 AnalogIn var(PC_0);
 Serial PC(USBTX,USBRX);
-
+ 
 int ser_spe[4] = {32,32,32,32};
 int ser_pos[4] = {135,135,135,135};
 int now_pos[4] = {};
-
+ 
 int prePos = 2000;
 int behPos = 1700;
 int upPos = 850;
 int downPos = 1350;
-
+ 
 float fmap(float x, float in_min, float in_max, float out_min, float out_max){
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
-
-
+ 
+ 
 void serServoMove(int num,int angle,int speed){
     ser_spe[num] = speed;
     ser_pos[num] = angle;
 }
-
+ 
 void bluepill_rx(){
     if(SerialServo.readable()){
         int request = SerialServo.getc();
@@ -64,49 +64,49 @@
     */
     
 }
-
+ 
 void RFmove(int position,int speed){
     if(2350 <= position){
         position = 2350;
     }
-    RFelbow.SpeedConvert(speed);
-    RFelbow.SetPosition(fmap(position,850,2350,2350,850));
+    //RFelbow.SpeedConvert(speed);
+    RFelbow.SetPosition(fmap(position,850,2350,2350,850),speed);
 }
-
+ 
 void LFmove(int position,int speed){
     if(2350 <= position){
         position = 2350;
     }
     
-    LFelbow.SpeedConvert(speed);
-    LFelbow.SetPosition(position);
+    //LFelbow.SpeedConvert(speed);
+    LFelbow.SetPosition(position,speed);
 }
-
+ 
 void RBmove(int position,int speed){
     if(2350 <= position){
         position = 2350;
     }
     
-    RBelbow.SpeedConvert(speed);
-    RBelbow.SetPosition(position);
+    //RBelbow.SpeedConvert(speed);
+    RBelbow.SetPosition(position,speed);
 }
-
+ 
 void LBmove(int position,int speed){
     if(2350 <= position){
         position = 2350;
     }
     
-    LBelbow.SpeedConvert(speed);
-    LBelbow.SetPosition(fmap(position,850,2350,2350,850));
+    //LBelbow.SpeedConvert(speed);
+    LBelbow.SetPosition(fmap(position,850,2350,2350,850),speed);
 }
-
+ 
 void forwarding(){
     RFmove(1600,3);
     LFmove(1600,3);
     RBmove(2200,3);
     LBmove(2200,3);
 }
-
+ 
 float angle = 0;
 int main() {
     //初期設定
@@ -117,12 +117,12 @@
     LFelbow.Enable(behPos,330,10);
     RBelbow.Enable(prePos,330,10);
     LBelbow.Enable(fmap(prePos,850,2350,2350,850),330,10);
-
+ 
     RFknee.Enable(downPos,330,50);
     LFknee.Enable(downPos,330,50);
     RBknee.Enable(downPos-100,330,50);
     LBknee.Enable(downPos-100,330,50);
-
+ 
     serServoMove(0,135,100);
     serServoMove(1,135,100);
     serServoMove(2,135,100);
@@ -130,31 +130,60 @@
     forwarding();
     wait(5);
     while (true) {
-        angle = int(20*var);
-        PC.printf("%f \n",angle);        
-        centerMove(angle,0);
-        RBknee.SetPosition(downPos -(var*300));
-        LBknee.SetPosition(downPos -(var*300));
+        //angle = int(20*var);
+        //PC.printf("%f \n",angle);        
+        RBknee.SetPosition(downPos - 300,10);
+        LBknee.SetPosition(downPos - 300,10);
         
         if(Dmode == true){
-            //RFmove(prePos,50);
-            RFknee.SetPosition(upPos);
+            /*
+            centerMove(20,0);
+            wait(2.0);
+            RBknee.SetPosition(upPos-100,10);
+            wait(2.0);
+            RBmove(behPos,25);
             wait(2.0);
-            RFmove(2350,10);
+            //RFmove(2350,10);
+            //wait(2.0);
+            RFknee.SetPosition(downPos-100,10);
             wait(2.0);
-            RFknee.SetPosition(downPos);
+            */
+            
+            centerMove(20,0);
+            wait(2.0);
+            RFknee.SetPosition(upPos,10);
             wait(2.0);
             RFmove(prePos,25);
             wait(2.0);
+            //RFmove(2350,10);
+            //wait(2.0);
+            RFknee.SetPosition(downPos,10);
+            wait(2.0);
+            
+            centerMove(-20,0);
+            wait(2.0);
+            LFknee.SetPosition(upPos,10);
+            wait(2.0);
+            LFmove(prePos,25);
+            wait(2.0);
+            LFknee.SetPosition(downPos,10);
+            wait(2.0);
+            
             centerMove(0,0);
-            RBknee.SetPosition(downPos);
-            LBknee.SetPosition(downPos);
+            wait(2.0);
             
-            RBknee.SetPosition(downPos);
-            LBknee.SetPosition(downPos);
+            RBknee.SetPosition(downPos,10);
+            LBknee.SetPosition(downPos,10);
+            RBknee.SetPosition(downPos-300,10);
+            LBknee.SetPosition(downPos-300,10);
+            
+            //RBknee.SpeedConvert(10);
+            //LBknee.SpeedConvert(10);
+            wait(3.0);
+            
             
         }else{
-            RFknee.SetPosition(downPos);
+            RFknee.SetPosition(downPos,10);
             forwarding();
         }