Ohnishi_Gundan / Mbed 2 deprecated Master-FM

FM-test

Dependencies:   MODSERIAL mbed-rtos mbed

Fork of Master by Ohnishi_Gundan

control.cpp@13:3f0505bbe284, 2014-09-15 (annotated)

Committer:
9uS7
Date:
Mon Sep 15 04:05:56 2014 +0000
Revision:
13:3f0505bbe284
Parent:
9:6057314dc8ec 
add fmStop and fmRestart

Who changed what in which revision?

UserRevisionLine numberNew contents of line
9uS7 0:4f07ba929908 1 #include "mbed.h"
9uS7 0:4f07ba929908 2 #include "control.h"
9uS7 0:4f07ba929908 3
9uS7 1:e1cfb5850088 4 //sensor
9uS7 8:bfcfda6b38fe 5 AnalogIn IR(p20);
9uS7 8:bfcfda6b38fe 6 AnalogIn FSR(p19);
9uS7 1:e1cfb5850088 7
9uS7 1:e1cfb5850088 8 //motor
9uS7 1:e1cfb5850088 9 DigitalOut Sig1(p21);
9uS7 1:e1cfb5850088 10 DigitalOut Sig2(p23);
9uS7 1:e1cfb5850088 11 PwmOut Pwm(p22);
9uS7 1:e1cfb5850088 12
9uS7 8:bfcfda6b38fe 13 RawSerial c_pc(USBTX, USBRX); // tx, rx
9uS7 8:bfcfda6b38fe 14
9uS7 2:c610e1a7fbcd 15 void motorSetup(void){
9uS7 2:c610e1a7fbcd 16 resetPos();
9uS7 2:c610e1a7fbcd 17 }
9uS7 2:c610e1a7fbcd 18
9uS7 3:12e1f116ea42 19 void motor( char function , float power ){
9uS7 3:12e1f116ea42 20 switch(function){
9uS7 3:12e1f116ea42 21 case MOTOR_PULL :
9uS7 3:12e1f116ea42 22 pull( power );
9uS7 3:12e1f116ea42 23 break;
9uS7 3:12e1f116ea42 24 case MOTOR_LOOSE :
9uS7 3:12e1f116ea42 25 loose( power );
9uS7 3:12e1f116ea42 26 break;
9uS7 3:12e1f116ea42 27 case MOTOR_OPEN:
9uS7 3:12e1f116ea42 28 open();
9uS7 3:12e1f116ea42 29 break;
9uS7 3:12e1f116ea42 30 case MOTOR_BRAKE:
9uS7 3:12e1f116ea42 31 brake();
9uS7 3:12e1f116ea42 32 break;
9uS7 3:12e1f116ea42 33 }
9uS7 3:12e1f116ea42 34 }
9uS7 3:12e1f116ea42 35
9uS7 0:4f07ba929908 36 void pull(float buf){
9uS7 9:6057314dc8ec 37 Sig1 = 1;
9uS7 9:6057314dc8ec 38 Sig2 = 0;
9uS7 0:4f07ba929908 39 Pwm = zeroPWM+buf/3;
9uS7 0:4f07ba929908 40 }
9uS7 0:4f07ba929908 41
9uS7 0:4f07ba929908 42 void loose(float buf){
9uS7 9:6057314dc8ec 43 Sig1 = 0;
9uS7 9:6057314dc8ec 44 Sig2 = 1;
9uS7 0:4f07ba929908 45 Pwm = zeroPWM+buf/3;
9uS7 0:4f07ba929908 46 }
9uS7 0:4f07ba929908 47
9uS7 0:4f07ba929908 48 void brake(void){
9uS7 0:4f07ba929908 49 Sig1 = 0;
9uS7 0:4f07ba929908 50 Sig2 = 0;
9uS7 0:4f07ba929908 51 Pwm = 1;
9uS7 0:4f07ba929908 52 }
9uS7 0:4f07ba929908 53
9uS7 0:4f07ba929908 54 void open(void){
9uS7 0:4f07ba929908 55 Sig1 = 0;
9uS7 0:4f07ba929908 56 Sig2 = 0;
9uS7 0:4f07ba929908 57 Pwm = 0;
9uS7 0:4f07ba929908 58 }
9uS7 0:4f07ba929908 59
9uS7 2:c610e1a7fbcd 60 void resetPos(void){
9uS7 0:4f07ba929908 61 int pos_flag=0;
9uS7 1:e1cfb5850088 62
9uS7 0:4f07ba929908 63 while(1){
9uS7 1:e1cfb5850088 64 if(IR>center_pos){
9uS7 0:4f07ba929908 65 pull(0.3);
9uS7 1:e1cfb5850088 66 }else if(IR<center_pos){
9uS7 0:4f07ba929908 67 loose(0.3);
9uS7 0:4f07ba929908 68 }
9uS7 1:e1cfb5850088 69 if(abs(IR-center_pos)<0.1){
9uS7 1:e1cfb5850088 70 open();
9uS7 1:e1cfb5850088 71 pos_flag=-1;
9uS7 1:e1cfb5850088 72 }
9uS7 1:e1cfb5850088 73 if(pos_flag) break;
9uS7 0:4f07ba929908 74 }
9uS7 3:12e1f116ea42 75 }
9uS7 3:12e1f116ea42 76
9uS7 3:12e1f116ea42 77
9uS7 3:12e1f116ea42 78 void getSensor(float* _ir,float* _fsr){
9uS7 3:12e1f116ea42 79 *_ir = IR;
9uS7 3:12e1f116ea42 80 *_fsr = FSR;
9uS7 0:4f07ba929908 81 }