code for basic movement of robot
Dependencies: MODSERIAL QEI mbed
main.cpp
- Committer:
- Technical_Muffin
- Date:
- 2015-10-09
- Revision:
- 7:46d9f01afba2
- Parent:
- 6:206fff3b20b0
- Child:
- 8:e0fc6dd187a2
File content as of revision 7:46d9f01afba2:
#include "mbed.h" #include "QEI.h" #include "MODSERIAL.h" //#include <"math.h"> QEI motor1(D13,D12,NC, 624); MODSERIAL pc(USBTX,USBRX); DigitalOut direction1(D7); PwmOut speed1(D6); DigitalIn button1(PTC6); DigitalOut led1(LED_RED); DigitalOut led2(LED_BLUE); DigitalOut led3(LED_GREEN); int main() { float cycle = 0.7f;//define the speed of the motor bool motor1_on = 1; int motor1_dir=0; while(1){ led3.write(0); led1.write(1); led2.write(1); speed1.write(0); pc.baud(115200); int diffa = button1.read(); wait(0.2); int diffb = button1.read(); int button_toggle = diffa-diffb; if(button_toggle == 1 && motor1_dir == 1){ motor1_dir = 0; } else if(button_toggle == 1 && motor1_dir == 0){ motor1_dir = 1; } while(button1.read() != motor1_on){// turn on motor 1 when the button is being pressed led3.write(1); led1.write(0); speed1.write(cycle);//write the speed to the motor speed1.period_us(100);//Set period of PWM to 100 us. pc.printf("%f",speed1.read());//klopt nog niet, maar voorlopig zorgt het uitlezen ervoor dat het werkt. direction1.write(motor1_dir);//turn motor CCW or CW //motor CW = 0 //motor CCW = 1 } } } //determine speed of movement from the encoder signal // the amount of counts for one revolution is 32 //this is X2 encodng, as the QEI uses X2 by default //and the motor encoder has a X4 encoder and thus 64 counts per revolution //thus keeping in mind the gearbox with 1:131,25 gear ratio: // the amount of counts for a gearbox revolution or resolution is 8400 // a revolution is 2 pi rad, resolution is : 8400/2 pi = 1336.90 counts/rad //or for the encoder x2 its 668,45 counts/rad /* Rotational position in degrees can be calculated by: * * (pulse count / X * N) * 360 * * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number * of pulses per revolution. * * Linear position can be calculated by: * * (pulse count / X * N) * (1 / PPI) * * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of * pulses per revolution, and PPI is pulses per inch, or the equivalent for * any other unit of displacement. PPI can be calculated by taking the * circumference of the wheel or encoder disk and dividing it by the number * of pulses per revolution. * Reset the encoder. * * Sets the pulses and revolutions count to zero. void reset(void); */