Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "mbed.h"
- #include "C12832.h"
- DigitalOut Enable(PB_2);
- Ticker leftWheelTime;
- Ticker rightWheelTime;
- InterruptIn leftEncoderPin(PB_12);
- InterruptIn rightEncoderPin(PC_14);
- C12832 lcd(D11, D13, D12, D7, D10);
- class encoder{
- private:
- PwmOut motor;
- DigitalOut motorDirection;
- DigitalOut motorMode;
- int edgeCount;
- double velocity, totalDistance, deltaDistance, tickRate;
- public:
- encoder (PinName pwmPin, PinName dirPin, PinName modePin):motor(pwmPin), motorDirection(dirPin), motorMode(modePin){
- edgeCount = 0;
- velocity = 0;
- totalDistance = 0;
- deltaDistance = 0;
- tickRate = 0;
- motor = 0;
- motorMode = 0;
- }
- void encoderCount(){
- edgeCount++;
- }
- void speedCalc(){
- tickRate = edgeCount/0.1;
- velocity = (tickRate/256)*0.08*3.14;
- totalDistance = velocity*0.1;
- edgeCount = 0;
- }
- void setMotor(int period, int direction){
- motor.period_ms(period);
- motorDirection.write(direction);
- //motorMode.write(mode);
- }
- void setMotorDirection(int direction){
- motorDirection.write(direction);
- }
- void setMotorDutyCycle(float dutyCycle){
- motor.write(dutyCycle);
- }
- float velocityReturn(){
- return velocity;
- }
- float distReturn(){
- return totalDistance;
- }
- };
- class distanceCalculator{
- private:
- bool turn;
- int distCount;
- float totalDistance, rotationDistance, incDistance;
- public:
- distanceCalculator () {
- totalDistance = 0;
- rotationDistance = 0;
- incDistance = 0;
- distCount = 0;
- }
- /* void initializetotalDistance(){
- totalDistance = 0;
- rotationDistance = 0;
- } */
- int totDistance(float leftDistance, float rightDistance){
- if (turn == false){
- incDistance = ((leftDistance + rightDistance)/2) + incDistance;
- if (incDistance >= 0.5){
- distCount++;
- incDistance =0;
- }
- totalDistance = ((leftDistance + rightDistance)/2) + totalDistance;
- }
- return distCount;
- }
- double rotDistance(double leftVelocity, double rightVelocity){
- if (turn == true){
- rotationDistance = (((leftVelocity+rightVelocity)/0.083)*0.1)+ rotationDistance; //changed 0.01 to 0.1
- }
- return rotationDistance;
- }
- void clearRotDist(){
- rotationDistance = 0;
- }
- void turnState (bool state){
- turn = state;
- }
- int getturn(){ //check of turn
- return turn;
- }
- };
- class guidanceSM{
- private:
- // int leftCount,rightCount;
- int state, nextstate;
- encoder* lEncoder;
- encoder* rEncoder;
- distanceCalculator* calc;
- int turnCount;
- bool turn;
- public:
- guidanceSM (encoder* LE, encoder* RE, distanceCalculator* d): lEncoder(LE), rEncoder(RE), calc(d){
- turnCount = 0;
- state = 0;
- nextstate = 0;
- // leftCount = 0;
- // rightCount = 0;
- turn = false;
- }
- /* void LCount(){
- if (turn == true) {leftCount++;}
- else {leftCount = 0;}
- }
- void RCount(){
- if (turn == true) {rightCount++;}
- else {rightCount = 0;}
- }*/
- int getturn(){ //check of turn
- return calc->getturn();
- }
- int getturnCount(){ //check of count
- return turnCount;
- }
- /* float getreturnRotDist(){ //check of rotdist
- return calc->rotDistance(lSpeed, rSpeed);
- }*/
- void stateMachine(int distCount, float lSpeed, float rSpeed){
- state = getState(distCount);
- if (state == 1){ //drive straight
- lEncoder->setMotorDirection(1);
- rEncoder->setMotorDirection(1);
- lEncoder->setMotorDutyCycle(0.8);
- rEncoder->setMotorDutyCycle(0.81);
- }
- if (state == 2){ //turn right
- lEncoder->setMotorDirection(1);
- rEncoder->setMotorDirection(0);
- lEncoder->setMotorDutyCycle(0.8);
- rEncoder->setMotorDutyCycle(0.81);
- // turn = true;
- calc->turnState(true);
- if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*15.5) { // During counting distance, we cannot using d=v*t, because the state machine run
- turnCount++; // with very high frequency, rotDistance will be count each cycle but not each 0.1 sec
- calc->clearRotDist(); // so what we can do is counting the risingedge.
- calc->turnState(false);
- // turn = false;
- }
- }
- if (state == 3){ //stop
- lEncoder->setMotorDutyCycle(1);
- rEncoder->setMotorDutyCycle(1);
- calc->clearRotDist();
- wait(0.5);
- }
- if (state == 4){ //turn left+left
- lEncoder->setMotorDirection(0);
- rEncoder->setMotorDirection(1);
- lEncoder->setMotorDutyCycle(0.8);
- rEncoder->setMotorDutyCycle(0.81);
- calc->turnState(true);
- if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*2*25) {
- turnCount++;
- calc->clearRotDist();
- calc->turnState(false);
- }
- }
- if (state == 5){ //turn left
- lEncoder->setMotorDirection(0);
- rEncoder->setMotorDirection(1);
- lEncoder->setMotorDutyCycle(0.8);
- rEncoder->setMotorDutyCycle(0.81);
- calc->turnState(true);
- if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*15.5) {
- turnCount++;
- calc->clearRotDist();
- calc->turnState(false);
- }
- }
- }
- int getState(int dist){
- switch (state){
- case (0): if (dist <=7){nextstate = 1;} else {Enable = 0, nextstate = 0;} break;
- case (1): if ((dist<1) or ((dist<2) and (turnCount==1)) or ((dist<3) and (turnCount==2)) or ((dist<4) and (turnCount==3)) or ((dist<5) and (turnCount==4))or ((dist<6) and (turnCount==5))or ((dist<7) and (turnCount==6))or ((dist<8) and (turnCount==7))){
- nextstate = 1;} else if (dist>=8){nextstate = 0;} else {nextstate = 3;} break;
- case (2): if (calc->getturn() == true) {nextstate = 2;} else {nextstate = 1;} break;
- case (3): if (turnCount < 3) {nextstate = 2;} else if (turnCount == 3) {nextstate = 4;} else {nextstate = 5;} break;
- case (4): if (calc->getturn() == true) {nextstate = 4;} else {nextstate = 1;} break;
- case (5): if (calc->getturn() == true) {nextstate = 5;} else {nextstate = 1;} break;
- }
- return nextstate;
- }
- };
- int main()
- {
- Enable = 0;
- encoder* leftEncoder = new encoder(PA_15,PC_8,PC_5); //PWM, direction, mode
- encoder* rightEncoder = new encoder(PB_14,PC_6,PC_4);
- distanceCalculator* d = new distanceCalculator;
- // guidanceSM* control = new guidanceSM(leftEncoder, rightEncoder, d, leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
- guidanceSM control(leftEncoder, rightEncoder, d);
- leftEncoder->setMotor(1,1);
- rightEncoder->setMotor(1,1);
- leftEncoderPin.rise(callback(leftEncoder,&encoder::encoderCount));
- rightEncoderPin.rise(callback(rightEncoder,&encoder::encoderCount));
- // leftEncoderPin.rise(callback(control,&guidanceSM::LCount));
- // rightEncoderPin.rise(callback(control,&guidanceSM::RCount));
- leftWheelTime.attach(callback(leftEncoder,&encoder::speedCalc), 0.1);
- rightWheelTime.attach(callback(rightEncoder,&encoder::speedCalc), 0.1);
- leftEncoder->setMotorDutyCycle(1);
- rightEncoder->setMotorDutyCycle(1);
- //d->initializetotalDistance();
- Enable = 1;
- while(1){
- wait(0.1);
- d->rotDistance(leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
- lcd.cls();
- lcd.locate(1,0);
- lcd.printf("LSpeed = %2.2f", rightEncoder->velocityReturn());
- // lcd.printf("RotDist = %2.2lf",control.getreturnRotDist());
- lcd.locate(1,10);
- // lcd.printf("turn = %d", control.getturn());
- lcd.printf("Distance = %d", d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn()));
- lcd.locate(1,20);
- // lcd.printf("turnCount = %d", control.getturnCount());
- lcd.printf("State = %d", control.getState(d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn())));
- control.stateMachine(d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn()), leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement