eLab Team / Mbed OS DcMotorTest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

final_v1_code.txt@2:72952132894a, 2021-02-20 (annotated)

Committer:
elab
Date:
Sat Feb 20 09:04:41 2021 +0000
Revision:
2:72952132894a
Version 1 of the smart marbles factory

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elab 2:72952132894a 1 #include "mbed.h"
elab 2:72952132894a 2 #include "stdio.h"
elab 2:72952132894a 3
elab 2:72952132894a 4 //DigitalOut led(LED1);
elab 2:72952132894a 5
elab 2:72952132894a 6 Serial pc(USBTX, USBRX);
elab 2:72952132894a 7 DigitalIn my_button(USER_BUTTON); // Use the user button to switch from control mode and auto mode
elab 2:72952132894a 8 DigitalIn end_stop(A5); // relais fin de course
elab 2:72952132894a 9
elab 2:72952132894a 10 PwmOut l1_dc_motor_cw(D2);
elab 2:72952132894a 11 PwmOut l1_dc_motor_ccw(D3);
elab 2:72952132894a 12 DigitalOut l1_servo_cw(D4);
elab 2:72952132894a 13 DigitalOut l1_servo_ccw(D5);
elab 2:72952132894a 14 PwmOut l2_dc_motor_cw(D6);
elab 2:72952132894a 15 PwmOut l2_dc_motor_ccw(D7);
elab 2:72952132894a 16 DigitalOut l2_servo_cw(D8);
elab 2:72952132894a 17 DigitalOut l2_servo_ccw(D9);
elab 2:72952132894a 18 PwmOut l3_dc_motor_cw(D10);
elab 2:72952132894a 19 PwmOut l3_dc_motor_ccw(D11);
elab 2:72952132894a 20 DigitalOut l3_servo_cw(D12);
elab 2:72952132894a 21 DigitalOut l3_servo_ccw(D13);
elab 2:72952132894a 22 int l1_dc_speed=2500;
elab 2:72952132894a 23
elab 2:72952132894a 24 void run_l1_dc_motor_cw(void){
elab 2:72952132894a 25 l1_dc_motor_cw.period_ms(20);
elab 2:72952132894a 26 l1_dc_motor_cw.pulsewidth_us(l1_dc_speed);//1800
elab 2:72952132894a 27 }
elab 2:72952132894a 28 void run_l1_dc_motor_ccw(void){
elab 2:72952132894a 29 l1_dc_motor_ccw.period_ms(20); // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
elab 2:72952132894a 30 l1_dc_motor_ccw.pulsewidth_us(l1_dc_speed);
elab 2:72952132894a 31 }
elab 2:72952132894a 32 void run_l2_dc_motor_cw(void){
elab 2:72952132894a 33 l2_dc_motor_cw.period_ms(20);
elab 2:72952132894a 34 l2_dc_motor_cw.pulsewidth_us(5000);
elab 2:72952132894a 35 }
elab 2:72952132894a 36 void run_l2_dc_motor_ccw(void){
elab 2:72952132894a 37 l2_dc_motor_ccw.period_ms(20); // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
elab 2:72952132894a 38 l2_dc_motor_ccw.pulsewidth_us(5000);
elab 2:72952132894a 39 }
elab 2:72952132894a 40 void run_l3_dc_motor_cw(void){
elab 2:72952132894a 41 // l3_dc_motor_cw.period_ms(20);
elab 2:72952132894a 42 // l3_dc_motor_cw.pulsewidth_us(10000);
elab 2:72952132894a 43 }
elab 2:72952132894a 44 void run_l3_dc_motor_ccw(void){
elab 2:72952132894a 45 l3_dc_motor_ccw.period_ms(20); // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
elab 2:72952132894a 46 l3_dc_motor_ccw.pulsewidth_us(5000);
elab 2:72952132894a 47 }
elab 2:72952132894a 48 void stop_dc_motor(void){
elab 2:72952132894a 49 l1_dc_motor_cw.pulsewidth_us(0);
elab 2:72952132894a 50 l1_dc_motor_ccw.pulsewidth_us(0);
elab 2:72952132894a 51 l2_dc_motor_cw.pulsewidth_us(0);
elab 2:72952132894a 52 l2_dc_motor_ccw.pulsewidth_us(0);
elab 2:72952132894a 53 l3_dc_motor_cw.pulsewidth_us(0);
elab 2:72952132894a 54 l3_dc_motor_ccw.pulsewidth_us(0);
elab 2:72952132894a 55 }
elab 2:72952132894a 56 void run_l1_servo_cw(void){
elab 2:72952132894a 57 l1_servo_cw=1;
elab 2:72952132894a 58 l1_servo_ccw=0;
elab 2:72952132894a 59 }
elab 2:72952132894a 60 void run_l1_servo_ccw(void){
elab 2:72952132894a 61 l1_servo_cw=0;
elab 2:72952132894a 62 l1_servo_ccw=1;
elab 2:72952132894a 63 }
elab 2:72952132894a 64 void run_l2_servo_cw(void){
elab 2:72952132894a 65 // l2_servo_cw=1;
elab 2:72952132894a 66 // l2_servo_ccw=0;
elab 2:72952132894a 67 }
elab 2:72952132894a 68 void run_l2_servo_ccw(void){
elab 2:72952132894a 69 l2_servo_cw=0;
elab 2:72952132894a 70 l2_servo_ccw=1;
elab 2:72952132894a 71 }
elab 2:72952132894a 72 void run_l3_servo_cw(void){
elab 2:72952132894a 73 l3_servo_cw=1;
elab 2:72952132894a 74 l3_servo_ccw=0;
elab 2:72952132894a 75 }
elab 2:72952132894a 76 void run_l3_servo_ccw(void){
elab 2:72952132894a 77 l3_servo_cw=0;
elab 2:72952132894a 78 l3_servo_ccw=1;
elab 2:72952132894a 79 }
elab 2:72952132894a 80 void stop_servo(void){
elab 2:72952132894a 81 l1_servo_cw=0;
elab 2:72952132894a 82 l1_servo_ccw=0;
elab 2:72952132894a 83 l2_servo_cw=0;
elab 2:72952132894a 84 l2_servo_ccw=0;
elab 2:72952132894a 85 l3_servo_cw=0;
elab 2:72952132894a 86 l3_servo_ccw=0;
elab 2:72952132894a 87 }
elab 2:72952132894a 88 void test_limite(void){
elab 2:72952132894a 89 end_stop.mode(PullDown);
elab 2:72952132894a 90 while(1){
elab 2:72952132894a 91 printf("lim=%d\n", end_stop.read());
elab 2:72952132894a 92
elab 2:72952132894a 93 if(end_stop.read()==1){
elab 2:72952132894a 94 stop_dc_motor();
elab 2:72952132894a 95 // led=0;
elab 2:72952132894a 96 printf("stop_dc_motor\n");
elab 2:72952132894a 97 }
elab 2:72952132894a 98 else{
elab 2:72952132894a 99 run_l2_dc_motor_ccw();
elab 2:72952132894a 100 printf("motor runs\n");
elab 2:72952132894a 101 // led=1;
elab 2:72952132894a 102 }
elab 2:72952132894a 103 }
elab 2:72952132894a 104
elab 2:72952132894a 105 }
elab 2:72952132894a 106 void auto_run(void){
elab 2:72952132894a 107 int l1_wait = 1300;
elab 2:72952132894a 108 int l2_wait = 1250;
elab 2:72952132894a 109 int l3_wait = 2500;
elab 2:72952132894a 110
elab 2:72952132894a 111 printf("Hello World !\n");
elab 2:72952132894a 112 run_l1_dc_motor_cw();
elab 2:72952132894a 113 wait_ms(l1_wait);
elab 2:72952132894a 114 stop_dc_motor();
elab 2:72952132894a 115 run_l1_dc_motor_ccw();
elab 2:72952132894a 116 wait_ms(l1_wait);
elab 2:72952132894a 117 stop_dc_motor();
elab 2:72952132894a 118 wait_ms(1000);
elab 2:72952132894a 119 run_l1_servo_cw();
elab 2:72952132894a 120 wait_ms(1000);
elab 2:72952132894a 121 stop_servo();
elab 2:72952132894a 122 run_l2_dc_motor_ccw();
elab 2:72952132894a 123 wait_ms(l2_wait);
elab 2:72952132894a 124 stop_dc_motor();
elab 2:72952132894a 125 wait_ms(1000);
elab 2:72952132894a 126 run_l2_servo_ccw();
elab 2:72952132894a 127 wait_ms(1000);
elab 2:72952132894a 128 stop_servo();
elab 2:72952132894a 129 run_l2_dc_motor_cw();
elab 2:72952132894a 130 wait_ms(l2_wait);
elab 2:72952132894a 131 stop_dc_motor();
elab 2:72952132894a 132 run_l1_dc_motor_cw();
elab 2:72952132894a 133 wait_ms(l1_wait);
elab 2:72952132894a 134 stop_dc_motor();
elab 2:72952132894a 135 run_l1_dc_motor_ccw();
elab 2:72952132894a 136 wait_ms(l1_wait);
elab 2:72952132894a 137 stop_dc_motor();
elab 2:72952132894a 138 wait_ms(1000);
elab 2:72952132894a 139 run_l1_servo_ccw();
elab 2:72952132894a 140 wait_ms(1000);
elab 2:72952132894a 141 stop_servo();
elab 2:72952132894a 142 run_l3_dc_motor_ccw();
elab 2:72952132894a 143 wait_ms(l3_wait);
elab 2:72952132894a 144 stop_dc_motor();
elab 2:72952132894a 145 }
elab 2:72952132894a 146
elab 2:72952132894a 147 void control_mode(void){
elab 2:72952132894a 148 unsigned char cmd=0;
elab 2:72952132894a 149 while(1){
elab 2:72952132894a 150 scanf("%c", &cmd);
elab 2:72952132894a 151 if(cmd == '1')
elab 2:72952132894a 152 {
elab 2:72952132894a 153 cmd = 0;
elab 2:72952132894a 154 stop_dc_motor();
elab 2:72952132894a 155 printf("stop_dc_motor\r\n");
elab 2:72952132894a 156 }
elab 2:72952132894a 157 if(cmd == '2')
elab 2:72952132894a 158 {
elab 2:72952132894a 159 cmd = 0;
elab 2:72952132894a 160 run_l1_dc_motor_cw();
elab 2:72952132894a 161 printf("run_l1_dc_motor_cw\r\n");
elab 2:72952132894a 162 }
elab 2:72952132894a 163 if(cmd == '3')
elab 2:72952132894a 164 {
elab 2:72952132894a 165 cmd = 0;
elab 2:72952132894a 166 run_l1_dc_motor_ccw();
elab 2:72952132894a 167 printf("run_l1_dc_motor_ccw\r\n");
elab 2:72952132894a 168 }
elab 2:72952132894a 169 if(cmd == '4')
elab 2:72952132894a 170 {
elab 2:72952132894a 171 cmd = 0;
elab 2:72952132894a 172 run_l1_servo_cw();
elab 2:72952132894a 173 wait(1);
elab 2:72952132894a 174 stop_servo();
elab 2:72952132894a 175 printf("run_l1_servo_cw\r\n");
elab 2:72952132894a 176 }
elab 2:72952132894a 177 if(cmd == '5')
elab 2:72952132894a 178 {
elab 2:72952132894a 179 cmd = 0;
elab 2:72952132894a 180 run_l1_servo_ccw();
elab 2:72952132894a 181 wait(1);
elab 2:72952132894a 182 stop_servo();
elab 2:72952132894a 183 printf("run_l1_servo_ccw\r\n");
elab 2:72952132894a 184 }
elab 2:72952132894a 185 if(cmd == '6')
elab 2:72952132894a 186 {
elab 2:72952132894a 187 cmd = 0;
elab 2:72952132894a 188 run_l2_dc_motor_cw();
elab 2:72952132894a 189 printf("run_l2_dc_motor_cw\r\n");
elab 2:72952132894a 190 }
elab 2:72952132894a 191 if(cmd == '7')
elab 2:72952132894a 192 {
elab 2:72952132894a 193 cmd = 0;
elab 2:72952132894a 194 run_l2_dc_motor_ccw();
elab 2:72952132894a 195 printf("run_l2_dc_motor_ccw\r\n");
elab 2:72952132894a 196 }
elab 2:72952132894a 197 if(cmd == '8')
elab 2:72952132894a 198 {
elab 2:72952132894a 199 cmd = 0;
elab 2:72952132894a 200 run_l2_servo_cw();
elab 2:72952132894a 201 wait(1);
elab 2:72952132894a 202 stop_servo();
elab 2:72952132894a 203 printf("run_l2_servo_cw\r\n");
elab 2:72952132894a 204 }
elab 2:72952132894a 205 if(cmd == '9')
elab 2:72952132894a 206 {
elab 2:72952132894a 207 cmd = 0;
elab 2:72952132894a 208 run_l2_servo_ccw();
elab 2:72952132894a 209 wait(1);
elab 2:72952132894a 210 stop_servo();
elab 2:72952132894a 211 printf("run_l2_servo_ccw\r\n");
elab 2:72952132894a 212 }
elab 2:72952132894a 213
elab 2:72952132894a 214 if(cmd == 'a')
elab 2:72952132894a 215 {
elab 2:72952132894a 216 cmd = 0;
elab 2:72952132894a 217 run_l3_dc_motor_cw();
elab 2:72952132894a 218 printf("run_l3_dc_motor_cw\r\n");
elab 2:72952132894a 219 }
elab 2:72952132894a 220 if(cmd == 'b')
elab 2:72952132894a 221 {
elab 2:72952132894a 222 cmd = 0;
elab 2:72952132894a 223 run_l3_dc_motor_ccw();
elab 2:72952132894a 224 printf("run_l3_dc_motor_ccw\r\n");
elab 2:72952132894a 225 }
elab 2:72952132894a 226 if(cmd == 'c')
elab 2:72952132894a 227 {
elab 2:72952132894a 228 cmd = 0;
elab 2:72952132894a 229 run_l3_servo_cw();
elab 2:72952132894a 230 wait(1);
elab 2:72952132894a 231 stop_servo();
elab 2:72952132894a 232 printf("run_l3_servo_cw\r\n");
elab 2:72952132894a 233 }
elab 2:72952132894a 234 if(cmd == 'd')
elab 2:72952132894a 235 {
elab 2:72952132894a 236 cmd = 0;
elab 2:72952132894a 237 run_l3_servo_ccw();
elab 2:72952132894a 238 wait(1);
elab 2:72952132894a 239 stop_servo();
elab 2:72952132894a 240 printf("run_l3_servo_ccw\r\n");
elab 2:72952132894a 241 }
elab 2:72952132894a 242 if(cmd == 't')
elab 2:72952132894a 243 {
elab 2:72952132894a 244 cmd = 0;
elab 2:72952132894a 245 test_limite();
elab 2:72952132894a 246 printf("test_limite()\r\n");
elab 2:72952132894a 247 }
elab 2:72952132894a 248
elab 2:72952132894a 249 if(cmd == 'e')
elab 2:72952132894a 250 {
elab 2:72952132894a 251 cmd = 0;
elab 2:72952132894a 252 printf("exit\r\n");
elab 2:72952132894a 253 break;
elab 2:72952132894a 254 }
elab 2:72952132894a 255
elab 2:72952132894a 256 if(cmd == 'h')
elab 2:72952132894a 257 {
elab 2:72952132894a 258 cmd=0;
elab 2:72952132894a 259 printf("1=stop_dc_motor\n");
elab 2:72952132894a 260 printf("2=run_l1_dc_motor_cw; 3=run_l1_dc_motor_ccw; 4=run_l1_servo_cw; 5=run_l1_servo_ccw\n");
elab 2:72952132894a 261 printf("6=run_l2_dc_motor_cw; 7=run_l2_dc_motor_ccw; 8=run_l2_servo_cw; 9=run_l2_servo_ccw\n");
elab 2:72952132894a 262 printf("a=run_l3_dc_motor_cw; b=run_l3_dc_motor_ccw; c=run_l3_servo_cw; d=run_l3_servo_ccw.\n");
elab 2:72952132894a 263 }
elab 2:72952132894a 264
elab 2:72952132894a 265 }
elab 2:72952132894a 266
elab 2:72952132894a 267 }
elab 2:72952132894a 268
elab 2:72952132894a 269 int main(){
elab 2:72952132894a 270 while(1){
elab 2:72952132894a 271 for (int i=0; i<3; i++) {
elab 2:72952132894a 272 auto_run();
elab 2:72952132894a 273 }
elab 2:72952132894a 274 control_mode();
elab 2:72952132894a 275 }
elab 2:72952132894a 276 }