Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
Diff: main.cpp
- Revision:
- 2:de7b6c1d67c4
- Parent:
- 1:b08ac32d1ddc
- Child:
- 3:156c3e536ed4
--- a/main.cpp Thu Oct 30 09:00:36 2014 +0000 +++ b/main.cpp Thu Oct 30 11:51:21 2014 +0000 @@ -73,8 +73,9 @@ int delta_pos_motor1_puls; void clamp(float * in, float min, float max); volatile bool looptimerflag; -uint16_t gewenste_snelheid = 2; -uint16_t gewenste_snelheid_rad = 4; +int16_t gewenste_snelheid = 2; +int16_t gewenste_snelheid_rad = 4; +int16_t gewenste_snelheid_rad_return = 1; float pid(float setpoint, float measurement); float pos_motor1_rad; float PWM1_percentage = 0; @@ -126,8 +127,8 @@ delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls) pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad) - snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen - snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen + //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen + //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen //scope.set(0, snelheid_motor1_rad); previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. @@ -136,6 +137,8 @@ if (current_pos_motor1 >= 400) { gewenste_snelheid_rad = 0; + current_herhalingen = previous_herhalingen + 1; + previous_herhalingen = current_herhalingen; } setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad; @@ -153,7 +156,7 @@ } else {} - if(PWM1_percentage < 0) + if(PWM1_percentage > 0) { motordir1 = 1; } @@ -165,7 +168,55 @@ pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); prev_setpoint = setpoint; - //scope.send(); + //scope.send(); +} + +void return2rust () { + current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; + current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is + delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls) + pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar + delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad) + //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen + //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen + //scope.set(0, snelheid_motor1_rad); + + previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. + + //nu gaan we positie regelen i.p.v. snelheid. + if (current_pos_motor1 <= 0) + { + gewenste_snelheid_rad_return = 0; + } + + setpoint = prev_setpoint + (TSAMP * gewenste_snelheid_rad_return); + /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ + PWM1_percentage = pid(setpoint, pos_motor1_rad); + //scope.set(1, setpoint-pos_motor1_rad); + + if (PWM1_percentage < -100) + { + PWM1_percentage = -100; + } + else if (PWM1_percentage >100) + { + PWM1_percentage =100; + } + else {} + + if(PWM1_percentage > 0) + { + motordir1 = 1; + } + else + { + motordir1 = 0; + } + + pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); + //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); + prev_setpoint = setpoint; + //scope.send(); } float pid(float setpoint, float measurement) @@ -193,9 +244,6 @@ *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c // *in = het getal dat staat op locatie van in --> waarde van new_pwm } - -void return2rust () { -} void statemachinefunction() { @@ -227,6 +275,10 @@ current_herhalingen = 0; previous_herhalingen = 0; state = EMG_KALIBRATIE; + motor1.setPosition(0); + motor2.setPosition(0); + pwm_motor1.period_us(100); + pwm_motor2.period_us(100); } break; } @@ -276,6 +328,7 @@ current_herhalingen = 0; previous_herhalingen = 0; } + break; } @@ -287,19 +340,23 @@ state = RETURN2RUST; current_herhalingen = 0; previous_herhalingen = 0; + prev_setpoint =0; + setpoint =0; + } - break; + break; } case RETURN2RUST: { return2rust(); - if (current_herhalingen == 200) + /*if (current_herhalingen == 200) { state = RUST; current_herhalingen = 0; previous_herhalingen = 0; } + */ break; } @@ -326,10 +383,6 @@ } int main() { - motor1.setPosition(0); - motor2.setPosition(0); - pwm_motor1.period_us(100); - pwm_motor2.period_us(100); statemachine.attach(&statemachinefunction, 0.005); // the address of the function to be attached (flip) and the interval (2 seconds) screen.attach(&screenupdate, 0.2); } \ No newline at end of file