Advertisement
Guest User

BuggySquareTD1

a guest
Feb 21st, 2019
89
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.96 KB | None | 0 0
  1. #include "mbed.h"
  2. #include "C12832.h"
  3.  
  4. DigitalOut Enable(PB_2);
  5. Ticker leftWheelTime;
  6. Ticker rightWheelTime;
  7.  
  8. InterruptIn leftEncoderPin(PB_12);
  9. InterruptIn rightEncoderPin(PC_14);
  10.  
  11. C12832 lcd(D11, D13, D12, D7, D10);
  12.  
  13. class encoder{
  14. private:
  15. PwmOut motor;
  16. DigitalOut motorDirection;
  17. DigitalOut motorMode;
  18. int edgeCount;
  19. double velocity, totalDistance, deltaDistance, tickRate;
  20. public:
  21. encoder (PinName pwmPin, PinName dirPin, PinName modePin):motor(pwmPin), motorDirection(dirPin), motorMode(modePin){
  22. edgeCount = 0;
  23. velocity = 0;
  24. totalDistance = 0;
  25. deltaDistance = 0;
  26. tickRate = 0;
  27. motor = 0;
  28. motorMode = 0;
  29. }
  30. void encoderCount(){
  31. edgeCount++;
  32. }
  33. void speedCalc(){
  34. tickRate = edgeCount/0.1;
  35. velocity = (tickRate/256)*0.08*3.14;
  36. totalDistance = velocity*0.1;
  37. edgeCount = 0;
  38. }
  39. void setMotor(int period, int direction){
  40. motor.period_ms(period);
  41. motorDirection.write(direction);
  42. //motorMode.write(mode);
  43. }
  44. void setMotorDirection(int direction){
  45. motorDirection.write(direction);
  46. }
  47. void setMotorDutyCycle(float dutyCycle){
  48. motor.write(dutyCycle);
  49. }
  50. float velocityReturn(){
  51. return velocity;
  52. }
  53. float distReturn(){
  54. return totalDistance;
  55. }
  56.  
  57. };
  58.  
  59. class distanceCalculator{
  60. private:
  61. bool turn;
  62. int distCount;
  63. float totalDistance, rotationDistance, incDistance;
  64. public:
  65. distanceCalculator () {
  66. totalDistance = 0;
  67. rotationDistance = 0;
  68. incDistance = 0;
  69. distCount = 0;
  70. }
  71. /* void initializetotalDistance(){
  72. totalDistance = 0;
  73. rotationDistance = 0;
  74. } */
  75. int totDistance(float leftDistance, float rightDistance){
  76. if (turn == false){
  77. incDistance = ((leftDistance + rightDistance)/2) + incDistance;
  78. if (incDistance >= 0.5){
  79. distCount++;
  80. incDistance =0;
  81. }
  82. totalDistance = ((leftDistance + rightDistance)/2) + totalDistance;
  83. }
  84. return distCount;
  85. }
  86. double rotDistance(double leftVelocity, double rightVelocity){
  87. if (turn == true){
  88. rotationDistance = (((leftVelocity+rightVelocity)/0.083)*0.1)+ rotationDistance; //changed 0.01 to 0.1
  89. }
  90. return rotationDistance;
  91. }
  92. void clearRotDist(){
  93. rotationDistance = 0;
  94. }
  95. void turnState (bool state){
  96. turn = state;
  97. }
  98. int getturn(){ //check of turn
  99. return turn;
  100. }
  101. };
  102. class guidanceSM{
  103. private:
  104. // int leftCount,rightCount;
  105. int state, nextstate;
  106. encoder* lEncoder;
  107. encoder* rEncoder;
  108. distanceCalculator* calc;
  109. int turnCount;
  110. bool turn;
  111. public:
  112. guidanceSM (encoder* LE, encoder* RE, distanceCalculator* d): lEncoder(LE), rEncoder(RE), calc(d){
  113. turnCount = 0;
  114. state = 0;
  115. nextstate = 0;
  116. // leftCount = 0;
  117. // rightCount = 0;
  118. turn = false;
  119. }
  120. /* void LCount(){
  121. if (turn == true) {leftCount++;}
  122. else {leftCount = 0;}
  123. }
  124. void RCount(){
  125. if (turn == true) {rightCount++;}
  126. else {rightCount = 0;}
  127. }*/
  128. int getturn(){ //check of turn
  129. return calc->getturn();
  130. }
  131. int getturnCount(){ //check of count
  132. return turnCount;
  133. }
  134. /* float getreturnRotDist(){ //check of rotdist
  135. return calc->rotDistance(lSpeed, rSpeed);
  136. }*/
  137. void stateMachine(int distCount, float lSpeed, float rSpeed){
  138. state = getState(distCount);
  139. if (state == 1){ //drive straight
  140. lEncoder->setMotorDirection(1);
  141. rEncoder->setMotorDirection(1);
  142. lEncoder->setMotorDutyCycle(0.8);
  143. rEncoder->setMotorDutyCycle(0.81);
  144. }
  145.  
  146. if (state == 2){ //turn right
  147. lEncoder->setMotorDirection(1);
  148. rEncoder->setMotorDirection(0);
  149. lEncoder->setMotorDutyCycle(0.8);
  150. rEncoder->setMotorDutyCycle(0.81);
  151. // turn = true;
  152. calc->turnState(true);
  153. if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*15.5) { // During counting distance, we cannot using d=v*t, because the state machine run
  154. turnCount++; // with very high frequency, rotDistance will be count each cycle but not each 0.1 sec
  155. calc->clearRotDist(); // so what we can do is counting the risingedge.
  156. calc->turnState(false);
  157. // turn = false;
  158. }
  159. }
  160.  
  161. if (state == 3){ //stop
  162. lEncoder->setMotorDutyCycle(1);
  163. rEncoder->setMotorDutyCycle(1);
  164. calc->clearRotDist();
  165. wait(0.5);
  166. }
  167.  
  168. if (state == 4){ //turn left+left
  169. lEncoder->setMotorDirection(0);
  170. rEncoder->setMotorDirection(1);
  171. lEncoder->setMotorDutyCycle(0.8);
  172. rEncoder->setMotorDutyCycle(0.81);
  173. calc->turnState(true);
  174. if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*2*25) {
  175. turnCount++;
  176. calc->clearRotDist();
  177. calc->turnState(false);
  178. }
  179. }
  180.  
  181. if (state == 5){ //turn left
  182. lEncoder->setMotorDirection(0);
  183. rEncoder->setMotorDirection(1);
  184. lEncoder->setMotorDutyCycle(0.8);
  185. rEncoder->setMotorDutyCycle(0.81);
  186. calc->turnState(true);
  187. if (calc->rotDistance(lSpeed, rSpeed) >= 0.065*15.5) {
  188. turnCount++;
  189. calc->clearRotDist();
  190. calc->turnState(false);
  191. }
  192. }
  193.  
  194.  
  195. }
  196. int getState(int dist){
  197. switch (state){
  198. case (0): if (dist <=7){nextstate = 1;} else {Enable = 0, nextstate = 0;} break;
  199. 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))){
  200. nextstate = 1;} else if (dist>=8){nextstate = 0;} else {nextstate = 3;} break;
  201. case (2): if (calc->getturn() == true) {nextstate = 2;} else {nextstate = 1;} break;
  202. case (3): if (turnCount < 3) {nextstate = 2;} else if (turnCount == 3) {nextstate = 4;} else {nextstate = 5;} break;
  203. case (4): if (calc->getturn() == true) {nextstate = 4;} else {nextstate = 1;} break;
  204. case (5): if (calc->getturn() == true) {nextstate = 5;} else {nextstate = 1;} break;
  205. }
  206.  
  207. return nextstate;
  208. }
  209. };
  210.  
  211. int main()
  212. {
  213. Enable = 0;
  214. encoder* leftEncoder = new encoder(PA_15,PC_8,PC_5); //PWM, direction, mode
  215. encoder* rightEncoder = new encoder(PB_14,PC_6,PC_4);
  216.  
  217. distanceCalculator* d = new distanceCalculator;
  218.  
  219. // guidanceSM* control = new guidanceSM(leftEncoder, rightEncoder, d, leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
  220. guidanceSM control(leftEncoder, rightEncoder, d);
  221. leftEncoder->setMotor(1,1);
  222. rightEncoder->setMotor(1,1);
  223. leftEncoderPin.rise(callback(leftEncoder,&encoder::encoderCount));
  224. rightEncoderPin.rise(callback(rightEncoder,&encoder::encoderCount));
  225.  
  226. // leftEncoderPin.rise(callback(control,&guidanceSM::LCount));
  227. // rightEncoderPin.rise(callback(control,&guidanceSM::RCount));
  228.  
  229. leftWheelTime.attach(callback(leftEncoder,&encoder::speedCalc), 0.1);
  230. rightWheelTime.attach(callback(rightEncoder,&encoder::speedCalc), 0.1);
  231. leftEncoder->setMotorDutyCycle(1);
  232. rightEncoder->setMotorDutyCycle(1);
  233. //d->initializetotalDistance();
  234. Enable = 1;
  235. while(1){
  236. wait(0.1);
  237. d->rotDistance(leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
  238. lcd.cls();
  239. lcd.locate(1,0);
  240. lcd.printf("LSpeed = %2.2f", rightEncoder->velocityReturn());
  241. // lcd.printf("RotDist = %2.2lf",control.getreturnRotDist());
  242. lcd.locate(1,10);
  243. // lcd.printf("turn = %d", control.getturn());
  244. lcd.printf("Distance = %d", d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn()));
  245. lcd.locate(1,20);
  246. // lcd.printf("turnCount = %d", control.getturnCount());
  247. lcd.printf("State = %d", control.getState(d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn())));
  248. control.stateMachine(d->totDistance(leftEncoder->distReturn(), rightEncoder->distReturn()), leftEncoder->velocityReturn(), rightEncoder->velocityReturn());
  249. }
  250.  
  251. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement