Ohnishi_Gundan / Mbed 2 deprecated Bilateral_SpringStyle2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:974404d7741b, 2014-10-10 (annotated)

Committer:
takeshi8931
Date:
Fri Oct 10 01:49:48 2014 +0000
Revision:
1:974404d7741b
Parent:
0:87273e4cfe1c 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeshi8931 0:87273e4cfe1c 1 #include "mbed.h"
takeshi8931 0:87273e4cfe1c 2
takeshi8931 0:87273e4cfe1c 3 #define fsr_def 0.05
takeshi8931 1:974404d7741b 4 #define sence_def 0.05
takeshi8931 0:87273e4cfe1c 5 #define zeroPWM 0.17
takeshi8931 1:974404d7741b 6 #define pullzero 0.95
takeshi8931 1:974404d7741b 7 #define powerlevel 0.8 //小さいほど強くなるよ
takeshi8931 0:87273e4cfe1c 8
takeshi8931 0:87273e4cfe1c 9
takeshi8931 0:87273e4cfe1c 10 AnalogIn FSR1(p19);
takeshi8931 0:87273e4cfe1c 11 AnalogIn FSR2(p20);
takeshi8931 0:87273e4cfe1c 12
takeshi8931 0:87273e4cfe1c 13
takeshi8931 0:87273e4cfe1c 14 DigitalOut Sig11(p24);
takeshi8931 0:87273e4cfe1c 15 DigitalOut Sig12(p26);
takeshi8931 0:87273e4cfe1c 16 PwmOut Pwm1(p25);
takeshi8931 0:87273e4cfe1c 17 DigitalOut Sig21(p21);
takeshi8931 0:87273e4cfe1c 18 DigitalOut Sig22(p23);
takeshi8931 0:87273e4cfe1c 19 PwmOut Pwm2(p22);
takeshi8931 0:87273e4cfe1c 20
takeshi8931 0:87273e4cfe1c 21 DigitalOut myled1(LED1);
takeshi8931 0:87273e4cfe1c 22 DigitalOut myled2(LED2);
takeshi8931 0:87273e4cfe1c 23
takeshi8931 0:87273e4cfe1c 24 Serial pc(USBTX, USBRX);
takeshi8931 0:87273e4cfe1c 25
takeshi8931 0:87273e4cfe1c 26 //引く
takeshi8931 0:87273e4cfe1c 27 void Pull1(float buf){
takeshi8931 0:87273e4cfe1c 28 Sig11 = 0;
takeshi8931 0:87273e4cfe1c 29 Sig12 = 1;
takeshi8931 1:974404d7741b 30 Pwm1 = 1.0-(powerlevel*buf*buf);
takeshi8931 0:87273e4cfe1c 31 //Pwm1 = 0.5;
takeshi8931 0:87273e4cfe1c 32 }
takeshi8931 0:87273e4cfe1c 33
takeshi8931 0:87273e4cfe1c 34 void Pull2(float buf){
takeshi8931 0:87273e4cfe1c 35 Sig21 = 0;
takeshi8931 0:87273e4cfe1c 36 Sig22 = 1;
takeshi8931 1:974404d7741b 37 Pwm2 = 1.0-(powerlevel*buf*buf);
takeshi8931 0:87273e4cfe1c 38 //Pwm2 = 0.5;
takeshi8931 0:87273e4cfe1c 39 }
takeshi8931 0:87273e4cfe1c 40
takeshi8931 0:87273e4cfe1c 41
takeshi8931 0:87273e4cfe1c 42
takeshi8931 0:87273e4cfe1c 43 //ブレーキモード
takeshi8931 0:87273e4cfe1c 44 void Brake1(){
takeshi8931 0:87273e4cfe1c 45 Sig11 = 0;
takeshi8931 0:87273e4cfe1c 46 Sig12 = 0;
takeshi8931 0:87273e4cfe1c 47 Pwm1 = 1;
takeshi8931 0:87273e4cfe1c 48 }
takeshi8931 0:87273e4cfe1c 49
takeshi8931 0:87273e4cfe1c 50 void Brake2(){
takeshi8931 0:87273e4cfe1c 51 Sig21 = 0;
takeshi8931 0:87273e4cfe1c 52 Sig22 = 0;
takeshi8931 0:87273e4cfe1c 53 Pwm2 = 1;
takeshi8931 0:87273e4cfe1c 54 }
takeshi8931 0:87273e4cfe1c 55
takeshi8931 0:87273e4cfe1c 56
takeshi8931 0:87273e4cfe1c 57 //開放
takeshi8931 0:87273e4cfe1c 58 void Open1(){
takeshi8931 0:87273e4cfe1c 59 Sig11 = 0;
takeshi8931 0:87273e4cfe1c 60 Sig12 = 0;
takeshi8931 0:87273e4cfe1c 61 Pwm1 = 0;
takeshi8931 0:87273e4cfe1c 62 }
takeshi8931 0:87273e4cfe1c 63
takeshi8931 0:87273e4cfe1c 64 void Open2(){
takeshi8931 0:87273e4cfe1c 65 Sig21 = 0;
takeshi8931 0:87273e4cfe1c 66 Sig22 = 0;
takeshi8931 0:87273e4cfe1c 67 Pwm2 = 0;
takeshi8931 0:87273e4cfe1c 68 }
takeshi8931 0:87273e4cfe1c 69
takeshi8931 0:87273e4cfe1c 70
takeshi8931 0:87273e4cfe1c 71 //FSRと相手側モータを回す
takeshi8931 0:87273e4cfe1c 72 void Moter2FSR1(float buf){
takeshi8931 0:87273e4cfe1c 73 if((FSR1-buf)>fsr_def && FSR2<pullzero){ //相手が引っ張ってる状態でピックってなる動き
takeshi8931 0:87273e4cfe1c 74 Pull2(FSR1);
takeshi8931 0:87273e4cfe1c 75 Brake1();
takeshi8931 0:87273e4cfe1c 76 myled1=1;
takeshi8931 0:87273e4cfe1c 77 }
takeshi8931 0:87273e4cfe1c 78 else if(FSR1<FSR2 && abs(FSR1-FSR2)>sence_def){//ギューんって引っ張る動き
takeshi8931 1:974404d7741b 79 Pull2(FSR1);
takeshi8931 0:87273e4cfe1c 80 Brake1();
takeshi8931 0:87273e4cfe1c 81 myled1=0;
takeshi8931 0:87273e4cfe1c 82 }
takeshi8931 0:87273e4cfe1c 83 else{
takeshi8931 0:87273e4cfe1c 84 Open2();
takeshi8931 0:87273e4cfe1c 85 myled1=0;
takeshi8931 0:87273e4cfe1c 86 }
takeshi8931 0:87273e4cfe1c 87 }
takeshi8931 0:87273e4cfe1c 88
takeshi8931 0:87273e4cfe1c 89 void Moter1FSR2(float buf){
takeshi8931 0:87273e4cfe1c 90 if((FSR2-buf)>fsr_def && FSR1<pullzero){
takeshi8931 0:87273e4cfe1c 91 Pull1(FSR2);
takeshi8931 0:87273e4cfe1c 92 Brake2();
takeshi8931 0:87273e4cfe1c 93 myled2=1;
takeshi8931 0:87273e4cfe1c 94 }
takeshi8931 0:87273e4cfe1c 95 else if(FSR2<FSR1&& abs(FSR1-FSR2)>sence_def){
takeshi8931 1:974404d7741b 96 Pull1(FSR2);
takeshi8931 0:87273e4cfe1c 97 Brake2();
takeshi8931 0:87273e4cfe1c 98 myled2=0;
takeshi8931 0:87273e4cfe1c 99 }
takeshi8931 0:87273e4cfe1c 100 else{
takeshi8931 0:87273e4cfe1c 101 Open1();
takeshi8931 0:87273e4cfe1c 102 myled2=0;
takeshi8931 0:87273e4cfe1c 103 }
takeshi8931 0:87273e4cfe1c 104 }
takeshi8931 0:87273e4cfe1c 105
takeshi8931 0:87273e4cfe1c 106
takeshi8931 0:87273e4cfe1c 107 int main() {
takeshi8931 0:87273e4cfe1c 108 float FSR1_buf = FSR1;
takeshi8931 0:87273e4cfe1c 109 float FSR2_buf = FSR2;
takeshi8931 0:87273e4cfe1c 110
takeshi8931 0:87273e4cfe1c 111
takeshi8931 0:87273e4cfe1c 112 Pwm1.period_us(50);
takeshi8931 0:87273e4cfe1c 113 Pwm2.period_us(50);
takeshi8931 0:87273e4cfe1c 114
takeshi8931 0:87273e4cfe1c 115 while(1) {
takeshi8931 0:87273e4cfe1c 116 pc.printf("FSR1:");
takeshi8931 0:87273e4cfe1c 117 pc.printf("%1.3f ",FSR1.read()); //圧力センサの出力
takeshi8931 0:87273e4cfe1c 118 pc.printf("FSR2:");
takeshi8931 0:87273e4cfe1c 119 pc.printf("%1.3f \n",FSR2.read()); //圧力センサの出力
takeshi8931 0:87273e4cfe1c 120
takeshi8931 0:87273e4cfe1c 121
takeshi8931 0:87273e4cfe1c 122 Moter2FSR1(FSR1_buf);
takeshi8931 0:87273e4cfe1c 123 Moter1FSR2(FSR2_buf);
takeshi8931 0:87273e4cfe1c 124
takeshi8931 0:87273e4cfe1c 125 FSR1_buf = FSR1;
takeshi8931 0:87273e4cfe1c 126 FSR2_buf = FSR2;
takeshi8931 0:87273e4cfe1c 127 }
takeshi8931 0:87273e4cfe1c 128 }