AresENSEA-CDF2020
/
AresCDFMainCode_capteur_US
Capteur_US
motors.cpp@10:0714feaaaee1, 2020-09-11 (annotated)
- Committer:
- Nanaud
- Date:
- Fri Sep 11 10:56:08 2020 +0000
- Revision:
- 10:0714feaaaee1
- Parent:
- 6:ea6b30c4bb01
11.09.20
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Nanaud | 1:2fe8c402ee79 | 1 | // Nom du fichier : motors.cpp |
Nanaud | 0:dc036b67c87c | 2 | #include "pins.h" |
Nanaud | 0:dc036b67c87c | 3 | |
Nanaud | 5:34ed652f8c31 | 4 | void drv_init() |
Nanaud | 5:34ed652f8c31 | 5 | { |
Nanaud | 0:dc036b67c87c | 6 | mot_dis(); |
Nanaud | 5:34ed652f8c31 | 7 | motGauche_fwd(); |
Nanaud | 6:ea6b30c4bb01 | 8 | //motDroite_fwd(); |
Nanaud | 6:ea6b30c4bb01 | 9 | motDroite_bck(); |
Nanaud | 10:0714feaaaee1 | 10 | drvGauche.moveLinSpeed(0.01); |
Nanaud | 10:0714feaaaee1 | 11 | drvDroite.moveLinSpeed(0.01); |
Nanaud | 10:0714feaaaee1 | 12 | //mode = 0b111; // M0, M1 et M2 sont à 1 |
Nanaud | 10:0714feaaaee1 | 13 | |
Nanaud | 10:0714feaaaee1 | 14 | mode_M0 = 1; |
Nanaud | 10:0714feaaaee1 | 15 | //mode_M1 = 1; |
Nanaud | 10:0714feaaaee1 | 16 | //mode_M2 = 1; |
Nanaud | 10:0714feaaaee1 | 17 | |
Nanaud | 10:0714feaaaee1 | 18 | //mot_en(); |
Nanaud | 0:dc036b67c87c | 19 | } |
Nanaud | 0:dc036b67c87c | 20 | |
Nanaud | 0:dc036b67c87c | 21 | // ENABLE/DISABLE // Les deux modules ont le même enable |
Nanaud | 10:0714feaaaee1 | 22 | |
Nanaud | 5:34ed652f8c31 | 23 | void mot_en() |
Nanaud | 5:34ed652f8c31 | 24 | { |
Nanaud | 10:0714feaaaee1 | 25 | motG_en(); |
Nanaud | 10:0714feaaaee1 | 26 | motD_en(); |
Nanaud | 0:dc036b67c87c | 27 | } |
Nanaud | 0:dc036b67c87c | 28 | |
Nanaud | 5:34ed652f8c31 | 29 | void mot_dis() |
Nanaud | 5:34ed652f8c31 | 30 | { |
Nanaud | 10:0714feaaaee1 | 31 | motG_dis(); |
Nanaud | 10:0714feaaaee1 | 32 | motD_dis(); |
Nanaud | 10:0714feaaaee1 | 33 | } |
Nanaud | 10:0714feaaaee1 | 34 | |
Nanaud | 10:0714feaaaee1 | 35 | void motG_en() |
Nanaud | 10:0714feaaaee1 | 36 | { |
Nanaud | 10:0714feaaaee1 | 37 | drvGauche.setEnable(START); |
Nanaud | 10:0714feaaaee1 | 38 | } |
Nanaud | 10:0714feaaaee1 | 39 | |
Nanaud | 10:0714feaaaee1 | 40 | void motD_en() |
Nanaud | 10:0714feaaaee1 | 41 | { |
Nanaud | 10:0714feaaaee1 | 42 | drvDroite.setEnable(START); |
Nanaud | 10:0714feaaaee1 | 43 | } |
Nanaud | 10:0714feaaaee1 | 44 | |
Nanaud | 10:0714feaaaee1 | 45 | void motG_dis() |
Nanaud | 10:0714feaaaee1 | 46 | { |
Nanaud | 0:dc036b67c87c | 47 | drvGauche.setEnable(STOP); |
Nanaud | 10:0714feaaaee1 | 48 | } |
Nanaud | 10:0714feaaaee1 | 49 | |
Nanaud | 10:0714feaaaee1 | 50 | void motD_dis() |
Nanaud | 10:0714feaaaee1 | 51 | { |
Nanaud | 10:0714feaaaee1 | 52 | drvDroite.setEnable(STOP); |
Nanaud | 0:dc036b67c87c | 53 | } |
Nanaud | 0:dc036b67c87c | 54 | |
Nanaud | 0:dc036b67c87c | 55 | // FORWARD |
Nanaud | 5:34ed652f8c31 | 56 | void motGauche_fwd() |
Nanaud | 5:34ed652f8c31 | 57 | { |
Nanaud | 0:dc036b67c87c | 58 | drvGauche.setDir(FORWARD); |
Nanaud | 10:0714feaaaee1 | 59 | //drvGauche.setDir(BACKWARD); |
Nanaud | 0:dc036b67c87c | 60 | } |
Nanaud | 0:dc036b67c87c | 61 | |
Nanaud | 5:34ed652f8c31 | 62 | void motDroite_fwd() |
Nanaud | 5:34ed652f8c31 | 63 | { |
Nanaud | 4:ad9b7355332e | 64 | drvDroite.setDir(BACKWARD); |
Nanaud | 10:0714feaaaee1 | 65 | //drvDroite.setDir(FORWARD); |
Nanaud | 0:dc036b67c87c | 66 | } |
Nanaud | 0:dc036b67c87c | 67 | |
Nanaud | 0:dc036b67c87c | 68 | // BACKWARD |
Nanaud | 5:34ed652f8c31 | 69 | void motGauche_bck() |
Nanaud | 5:34ed652f8c31 | 70 | { |
Nanaud | 0:dc036b67c87c | 71 | drvGauche.setDir(BACKWARD); |
Nanaud | 10:0714feaaaee1 | 72 | //drvGauche.setDir(FORWARD); |
Nanaud | 0:dc036b67c87c | 73 | } |
Nanaud | 0:dc036b67c87c | 74 | |
Nanaud | 5:34ed652f8c31 | 75 | void motDroite_bck() |
Nanaud | 5:34ed652f8c31 | 76 | { |
Nanaud | 4:ad9b7355332e | 77 | drvDroite.setDir(FORWARD); |
Nanaud | 10:0714feaaaee1 | 78 | //drvDroite.setDir(BACKWARD); |
Nanaud | 3:3ba377aafdfd | 79 | } |
Nanaud | 3:3ba377aafdfd | 80 | |
Nanaud | 3:3ba377aafdfd | 81 | |
Nanaud | 6:ea6b30c4bb01 | 82 | // FONCTIONS TESTS |
Nanaud | 6:ea6b30c4bb01 | 83 | // |
Nanaud | 5:34ed652f8c31 | 84 | void test_drv() |
Nanaud | 5:34ed652f8c31 | 85 | { |
Nanaud | 10:0714feaaaee1 | 86 | /* |
Nanaud | 3:3ba377aafdfd | 87 | mot_en(); |
Nanaud | 3:3ba377aafdfd | 88 | motGauche_fwd(); |
Nanaud | 4:ad9b7355332e | 89 | motDroite_fwd(); |
Nanaud | 5:34ed652f8c31 | 90 | drvGauche.moveLinSpeed(0.250); // 0.035 |
Nanaud | 5:34ed652f8c31 | 91 | drvDroite.moveLinSpeed(0.250); // 0.035 |
Nanaud | 3:3ba377aafdfd | 92 | wait(2); |
Nanaud | 3:3ba377aafdfd | 93 | motGauche_bck(); |
Nanaud | 4:ad9b7355332e | 94 | motDroite_bck(); |
Nanaud | 3:3ba377aafdfd | 95 | wait(2); |
Nanaud | 3:3ba377aafdfd | 96 | mot_dis(); |
Nanaud | 10:0714feaaaee1 | 97 | */ |
Nanaud | 10:0714feaaaee1 | 98 | |
Nanaud | 10:0714feaaaee1 | 99 | /* |
Nanaud | 10:0714feaaaee1 | 100 | mot_en(); |
Nanaud | 10:0714feaaaee1 | 101 | motGauche_fwd(); |
Nanaud | 10:0714feaaaee1 | 102 | motDroite_fwd(); |
Nanaud | 10:0714feaaaee1 | 103 | wait(2); |
Nanaud | 10:0714feaaaee1 | 104 | mot_dis(); |
Nanaud | 10:0714feaaaee1 | 105 | */ |
Nanaud | 10:0714feaaaee1 | 106 | |
Nanaud | 10:0714feaaaee1 | 107 | mot_en(); |
Nanaud | 10:0714feaaaee1 | 108 | mot_acc(); |
Nanaud | 10:0714feaaaee1 | 109 | wait(2); |
Nanaud | 10:0714feaaaee1 | 110 | mot_dec(); |
Nanaud | 10:0714feaaaee1 | 111 | mot_dis(); |
Nanaud | 10:0714feaaaee1 | 112 | |
Nanaud | 10:0714feaaaee1 | 113 | drvDroite.moveLinSpeed(0.050); |
Nanaud | 3:3ba377aafdfd | 114 | } |
Nanaud | 3:3ba377aafdfd | 115 | |
Nanaud | 10:0714feaaaee1 | 116 | void mot_acc() |
Nanaud | 10:0714feaaaee1 | 117 | { |
Nanaud | 10:0714feaaaee1 | 118 | double i = 0; |
Nanaud | 10:0714feaaaee1 | 119 | |
Nanaud | 10:0714feaaaee1 | 120 | while (i < vitesseSAT) { |
Nanaud | 10:0714feaaaee1 | 121 | bt.printf("mot_acc() => i = %lf\r\n",i); |
Nanaud | 10:0714feaaaee1 | 122 | drvDroite.moveLinSpeed(i); |
Nanaud | 10:0714feaaaee1 | 123 | drvGauche.moveLinSpeed(i); |
Nanaud | 10:0714feaaaee1 | 124 | i+=0.005; |
Nanaud | 10:0714feaaaee1 | 125 | wait_ms(10); |
Nanaud | 10:0714feaaaee1 | 126 | } |
Nanaud | 10:0714feaaaee1 | 127 | } |
Nanaud | 10:0714feaaaee1 | 128 | |
Nanaud | 10:0714feaaaee1 | 129 | void mot_dec() |
Nanaud | 10:0714feaaaee1 | 130 | { |
Nanaud | 10:0714feaaaee1 | 131 | double i = vitesseSAT; |
Nanaud | 10:0714feaaaee1 | 132 | |
Nanaud | 10:0714feaaaee1 | 133 | while (i > 0) { |
Nanaud | 10:0714feaaaee1 | 134 | bt.printf("mot_dec() => i = %lf\r\n",i); |
Nanaud | 10:0714feaaaee1 | 135 | drvDroite.moveLinSpeed(i); |
Nanaud | 10:0714feaaaee1 | 136 | drvGauche.moveLinSpeed(i); |
Nanaud | 10:0714feaaaee1 | 137 | i-=0.005; |
Nanaud | 10:0714feaaaee1 | 138 | wait_ms(10); |
Nanaud | 10:0714feaaaee1 | 139 | } |
Nanaud | 10:0714feaaaee1 | 140 | } |
Nanaud | 10:0714feaaaee1 | 141 | |
Nanaud | 10:0714feaaaee1 | 142 | void testAngle(int cmdAngle) {} |