Taiyo Koga / Mbed 2 deprecated teamA_saramawashi

Published late sorry

Dependencies:   mbed PS_PAD

main.cpp@0:50825421a4ce, 2020-08-26 (annotated)

Committer:
koga641028
Date:
Wed Aug 26 08:21:52 2020 +0000
Revision:
0:50825421a4ce
Child:
1:ef44f09e471e
madeteamA;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koga641028 0:50825421a4ce 1 /*
koga641028 0:50825421a4ce 2 * ======================
koga641028 0:50825421a4ce 3 * VS-C3/VS-RCV3 PIN
koga641028 0:50825421a4ce 4 * ======================
koga641028 0:50825421a4ce 5 * 1:NC 2:NC
koga641028 0:50825421a4ce 6 * 3:DAT 4:CMD
koga641028 0:50825421a4ce 7 * 5:SEL 6:CLK
koga641028 0:50825421a4ce 8 * 7:+5~7V 8:NC
koga641028 0:50825421a4ce 9 * 9:+3V 10:GND
koga641028 0:50825421a4ce 10 *
koga641028 0:50825421a4ce 11 */
koga641028 0:50825421a4ce 12
koga641028 0:50825421a4ce 13 #include "mbed.h"
koga641028 0:50825421a4ce 14 #include "PS_PAD.h"
koga641028 0:50825421a4ce 15
koga641028 0:50825421a4ce 16 class Motor{
koga641028 0:50825421a4ce 17 PwmOut a, b;
koga641028 0:50825421a4ce 18 public:
koga641028 0:50825421a4ce 19 Motor(PinName A_term, PinName B_term) : a(A_term), b(B_term) {
koga641028 0:50825421a4ce 20 a.period(0.001);
koga641028 0:50825421a4ce 21 b.period(0.001);
koga641028 0:50825421a4ce 22 }
koga641028 0:50825421a4ce 23
koga641028 0:50825421a4ce 24 Motor& operator=(double duty) {
koga641028 0:50825421a4ce 25 if(duty > 0){
koga641028 0:50825421a4ce 26 a = duty < 1 ? duty : 1;
koga641028 0:50825421a4ce 27 b = 0;
koga641028 0:50825421a4ce 28 }else {
koga641028 0:50825421a4ce 29 a = 0;
koga641028 0:50825421a4ce 30 b = duty > -1 ? -duty : -1;
koga641028 0:50825421a4ce 31 }
koga641028 0:50825421a4ce 32 return *this;
koga641028 0:50825421a4ce 33 }
koga641028 0:50825421a4ce 34 };
koga641028 0:50825421a4ce 35
koga641028 0:50825421a4ce 36
koga641028 0:50825421a4ce 37 PS_PAD vsc3(PA_7, PA_6, PA_5, PA_4); //MOSI, MISO, SCK, SS
koga641028 0:50825421a4ce 38
koga641028 0:50825421a4ce 39 DigitalIn lim[7] = {PD_2, PD_3, PD_4, PD_5, PD_6, PD_7, PG_9};
koga641028 0:50825421a4ce 40 Motor rmotor(PE_9, PE_11);
koga641028 0:50825421a4ce 41 Motor lmotor(PE_13, PE_14);
koga641028 0:50825421a4ce 42 Motor sxmotor(PA_0, PA_1);
koga641028 0:50825421a4ce 43 Motor tsmotor(PA_2, PA_3);
koga641028 0:50825421a4ce 44 Motor trmotor(PC_6, PC_7);
koga641028 0:50825421a4ce 45 Motor tlmotor(PC_8, PC_9);
koga641028 0:50825421a4ce 46
koga641028 0:50825421a4ce 47 int main() {
koga641028 0:50825421a4ce 48 vsc3.init();
koga641028 0:50825421a4ce 49 double movey = 0, turn = 0;
koga641028 0:50825421a4ce 50 printf("START\n");
koga641028 0:50825421a4ce 51
koga641028 0:50825421a4ce 52 while(1) {
koga641028 0:50825421a4ce 53 vsc3.poll();
koga641028 0:50825421a4ce 54 if (vsc3.read(PS_PAD::PAD_TOP)){
koga641028 0:50825421a4ce 55 movey = -1;
koga641028 0:50825421a4ce 56 }else if(vsc3.read(PS_PAD::PAD_BOTTOM)){
koga641028 0:50825421a4ce 57 movey = 1;
koga641028 0:50825421a4ce 58 }else if(abs(vsc3.read(PS_PAD::ANALOG_LY)) > 10){
koga641028 0:50825421a4ce 59 movey = (vsc3.read(PS_PAD::ANALOG_LY)) / 128.0;
koga641028 0:50825421a4ce 60 }else{
koga641028 0:50825421a4ce 61 movey = 0;
koga641028 0:50825421a4ce 62 }
koga641028 0:50825421a4ce 63
koga641028 0:50825421a4ce 64 if (vsc3.read(PS_PAD::PAD_RIGHT)){
koga641028 0:50825421a4ce 65 turn = -1;
koga641028 0:50825421a4ce 66 }else if(PS_PAD::PAD_LEFT){
koga641028 0:50825421a4ce 67 turn = 1;
koga641028 0:50825421a4ce 68 }else if(abs(vsc3.read(PS_PAD::ANALOG_LX)) > 10){
koga641028 0:50825421a4ce 69 turn = (vsc3.read(PS_PAD::ANALOG_LX)) / 128.0;
koga641028 0:50825421a4ce 70 }else{
koga641028 0:50825421a4ce 71 turn = 0;
koga641028 0:50825421a4ce 72 }
koga641028 0:50825421a4ce 73
koga641028 0:50825421a4ce 74 rmotor = -movey+turn; //move
koga641028 0:50825421a4ce 75 lmotor = -movey-turn;
koga641028 0:50825421a4ce 76
koga641028 0:50825421a4ce 77 if (vsc3.read(PS_PAD::PAD_CIRCLE) && lim[0] == 0){ //limitswitch
koga641028 0:50825421a4ce 78 sxmotor = 1;
koga641028 0:50825421a4ce 79 }else if(vsc3.read(PS_PAD::PAD_X) && lim[1] == 0){
koga641028 0:50825421a4ce 80 sxmotor = -1;
koga641028 0:50825421a4ce 81 }else if(vsc3.read(PS_PAD::PAD_CIRCLE) && vsc3.read(PS_PAD::PAD_X)){
koga641028 0:50825421a4ce 82 sxmotor = 0;
koga641028 0:50825421a4ce 83 }else{
koga641028 0:50825421a4ce 84 sxmotor = 0;
koga641028 0:50825421a4ce 85 }
koga641028 0:50825421a4ce 86
koga641028 0:50825421a4ce 87 if (vsc3.read(PS_PAD::PAD_TRIANGLE) && lim[2] == 0){
koga641028 0:50825421a4ce 88 tsmotor = 1;
koga641028 0:50825421a4ce 89 }else if(vsc3.read(PS_PAD::PAD_SQUARE) && lim[3] == 0){
koga641028 0:50825421a4ce 90 tsmotor = -1;
koga641028 0:50825421a4ce 91 }else if(vsc3.read(PS_PAD::PAD_TRIANGLE) && vsc3.read(PS_PAD::PAD_SQUARE)){
koga641028 0:50825421a4ce 92 tsmotor = 0;
koga641028 0:50825421a4ce 93 }else{
koga641028 0:50825421a4ce 94 tsmotor = 0;
koga641028 0:50825421a4ce 95 }
koga641028 0:50825421a4ce 96
koga641028 0:50825421a4ce 97 if (vsc3.read(PS_PAD::PAD_R1) && lim[4] == 0){
koga641028 0:50825421a4ce 98 trmotor = 1;
koga641028 0:50825421a4ce 99 }else if(vsc3.read(PS_PAD::PAD_R2) && lim[5] == 0){
koga641028 0:50825421a4ce 100 trmotor = -1;
koga641028 0:50825421a4ce 101 }else if(vsc3.read(PS_PAD::PAD_R1) && vsc3.read(PS_PAD::PAD_R2)){
koga641028 0:50825421a4ce 102 trmotor = 0;
koga641028 0:50825421a4ce 103 }else{
koga641028 0:50825421a4ce 104 trmotor = 0;
koga641028 0:50825421a4ce 105 }
koga641028 0:50825421a4ce 106
koga641028 0:50825421a4ce 107 if (vsc3.read(PS_PAD::PAD_L1) && lim[6] == 0){
koga641028 0:50825421a4ce 108 tlmotor = 1;
koga641028 0:50825421a4ce 109 }else if(vsc3.read(PS_PAD::PAD_L2) && lim[7] == 0){
koga641028 0:50825421a4ce 110 tlmotor = -1;
koga641028 0:50825421a4ce 111 }else if(vsc3.read(PS_PAD::PAD_L1) && vsc3.read(PS_PAD::PAD_L2)){
koga641028 0:50825421a4ce 112 tlmotor = 0;
koga641028 0:50825421a4ce 113 }else{
koga641028 0:50825421a4ce 114 tlmotor = 0;
koga641028 0:50825421a4ce 115 }
koga641028 0:50825421a4ce 116
koga641028 0:50825421a4ce 117 wait(0.05);
koga641028 0:50825421a4ce 118 }
koga641028 0:50825421a4ce 119 }