voor thomas
Dependencies: BMT-K9-Regelaar Encoder MODSERIAL mbed
Fork of BMT-K9-Regelaar by
main.cpp
- Committer:
- gerard1993
- Date:
- 2013-10-25
- Revision:
- 5:19687a179088
- Parent:
- 4:9ecf57487c72
File content as of revision 5:19687a179088:
#include "mbed.h" #include "encoder.h" #include "MODSERIAL.h" /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ void keep_in_range(float * in, float min, float max); volatile bool looptimerflag; void setlooptimerflag(void) { looptimerflag = true; } int main() { //LOCAL VARIABLES AnalogIn potmeter(PTC2); MODSERIAL pc(USBTX,USBRX); Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx Encoder motor2(PTD5,PTC8);//first pin on PTAx or PTDx PwmOut pwm_motor1(PTA12); PwmOut pwm_motor2(PTA5); DigitalOut motordir1(PTD3); DigitalOut motordir2(PTD1); pwm_motor1.period(1.0/22000.0); //MOTOR A float setpoint; float pwm_to_motor1; float setspeed; // float speed; // float position2; // float setpoint2; //MOTOR B float setpointB; float pwm_to_motor2; float setspeedB; float speedB; float position2B; float setpoint2B; //START OF CODE pc.baud(115200); Ticker looptimer; looptimer.attach(setlooptimerflag,0.001); //A // speed = 0; // position2 = 0; // setpoint = 0; //B speedB = 0; position2B = 0; setpoint2B = 0; //INFINITE LOOP while(1) { while(looptimerflag != true); looptimerflag = false; //MOTOR A setspeed = (potmeter.read()-0.5)*.0001; setpoint = setpoint + setspeed; pwm_to_motor1 = (setpoint - (motor1.getPosition()/4128))*20 + (setspeed - (motor1.getSpeed()/4128))*1.4 ; keep_in_range(&pwm_to_motor1, -1,1); if(pwm_to_motor1 > 0) motordir1 = 1; else motordir1 = 0; //WRITE VALUE TO MOTOR pwm_motor1.write(abs(pwm_to_motor1)); //MOTOR B setpointB = (potmeter.read()-0.5)*8000; setspeedB =(setpointB - setpoint2B)/0.01; speedB = (motor2.getPosition() - position2B)/0.001; pc.printf("s: %f, %d \n\r", setpointB, motor2.getPosition()); pwm_to_motor2 = (setpointB - motor2.getPosition())*.0001 + (setspeedB - speedB)*.00005 ; keep_in_range(&pwm_to_motor2, -1,1); setpoint2B = setpointB; position2B = motor2.getPosition(); if(pwm_to_motor2 > 0) motordir2 = 1; else motordir2 = 0; //WRITE VALUE TO MOTOR pwm_motor2.write(abs(pwm_to_motor2)); } } //coerces value 'in' to min or max when exceeding those values //if you'd like to understand the statement below take a google for //'ternary operators'. void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; }