FM-test
Dependencies: MODSERIAL mbed-rtos mbed
Fork of Master by
control.cpp
- Committer:
- 9uS7
- Date:
- 2014-09-15
- Revision:
- 13:3f0505bbe284
- Parent:
- 9:6057314dc8ec
File content as of revision 13:3f0505bbe284:
#include "mbed.h" #include "control.h" //sensor AnalogIn IR(p20); AnalogIn FSR(p19); //motor DigitalOut Sig1(p21); DigitalOut Sig2(p23); PwmOut Pwm(p22); RawSerial c_pc(USBTX, USBRX); // tx, rx void motorSetup(void){ resetPos(); } void motor( char function , float power ){ switch(function){ case MOTOR_PULL : pull( power ); break; case MOTOR_LOOSE : loose( power ); break; case MOTOR_OPEN: open(); break; case MOTOR_BRAKE: brake(); break; } } void pull(float buf){ Sig1 = 1; Sig2 = 0; Pwm = zeroPWM+buf/3; } void loose(float buf){ Sig1 = 0; Sig2 = 1; Pwm = zeroPWM+buf/3; } void brake(void){ Sig1 = 0; Sig2 = 0; Pwm = 1; } void open(void){ Sig1 = 0; Sig2 = 0; Pwm = 0; } void resetPos(void){ int pos_flag=0; while(1){ if(IR>center_pos){ pull(0.3); }else if(IR<center_pos){ loose(0.3); } if(abs(IR-center_pos)<0.1){ open(); pos_flag=-1; } if(pos_flag) break; } } void getSensor(float* _ir,float* _fsr){ *_ir = IR; *_fsr = FSR; }