Test to control motors
Dependencies: HIDScope_friso MODSERIAL QEI mbed
main.cpp
- Committer:
- FR150
- Date:
- 2016-10-24
- Revision:
- 2:8fb37a4587f1
- Parent:
- 0:42272569c45a
- Child:
- 3:62e950f20604
File content as of revision 2:8fb37a4587f1:
#include "mbed.h" #include "MODSERIAL.h" //for serial connection w/ PUTTY #define SERIAL_BAUD 115200 #include "QEI.h" //for encoder readout #include "HIDScope.h" //HIDScope MODSERIAL pc(USBTX, USBRX); //initiate serial connection QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); //Initiate encoder readout with pins D12, D13, 64 pulses per rotation and x4 encoding DigitalOut motor1_direction(D4); //Pin used to control motor direction PwmOut motor1_pwm(D5); //Pin used to control motor speed InterruptIn button1(D2); //button InterruptIn button2(D3); //button AnalogIn potmeter1(A0); //potmeter HIDScope scope(3); //initiate HIDScope with 2 channesl Ticker scopeTimer; //Ticker for HIDScope Timeout timeout; const float timer = 0.01; //refresh timer const float timer1 = 0.1; //refresh timer const float countspr = 8400; //counts per rotation of motor axis const float max_velocity = 5.2; //freespin max velocity of motor const float pi = 3.14159265359; //delicious pi int counts; //counts from encoder int counts_p; //previous counts from encoder int pwmvalue; //pwm value for motor control float buttoncount; float potmeter; //potmeter from 0-1 float referenceangle; float referenceangle_p = 0; float angle = 0; //angle of motor axis float angleerror; float referencevelocity; //controllable velocity float velocity; //calculated velocity from encoder void subtractcount(); void print(){ //print over serial connection pc.printf("%i\r\n", counts); pc.printf("%1.1f%\r\n", buttoncount); pc.printf("rangle: %3.3f%\r\n", referenceangle); pc.printf("angle: %3.3f%\r\n", angle); pc.printf("error: %3.3f%\r\n", angleerror); pc.printf("velocity: %3.3f%\r\n", velocity); } void controlmotor() { //set motor speed angleerror = referenceangle-angle; potmeter = potmeter1.read(); referencevelocity = potmeter*max_velocity; motor1_pwm.pulsewidth(referencevelocity/max_velocity*0.020); } void calculateangle() { //calculate the angle of the axis based on counts if (counts == 0) angle = 0; else angle = counts/countspr*2*pi; } void derivative() { //calculate velocity based on derivative velocity = (counts-counts_p)/countspr*2*pi/timer; counts_p = counts; } void scopeSend() {//send data over HIDScope scope.set(0,velocity); scope.set(1,angle); scope.set(2,referenceangle); scope.send(); } void calculatereferenceangle() { referenceangle = referenceangle_p+(buttoncount*pi/10); if(referenceangle < -pi) referenceangle = -pi; else if(referenceangle > pi) referenceangle = pi; referenceangle_p = referenceangle; buttoncount = 0; } void addcount() { buttoncount++; button1.rise(&addcount); button2.rise(&subtractcount); timeout.attach(calculatereferenceangle, 0.5); } void subtractcount() { buttoncount--; button1.rise(&addcount); button2.rise(&subtractcount); timeout.attach(calculatereferenceangle, 0.5); } int main() { pc.baud(SERIAL_BAUD); Ticker time; Ticker time1; Ticker time2; Ticker time3; time.attach(print, timer1); time1.attach(controlmotor, timer); time2.attach(calculateangle, timer); time3.attach(derivative, timer); button1.rise(&addcount); button2.rise(&subtractcount); scopeTimer.attach_us(&scopeSend, 1e4); motor1_pwm.period(0.020f); // set pwm period to 20ms while (true) { counts = Encoder.getPulses(); } }